AO-R167  871 
UNCLASSIFIED 


KALNAN  FILTER  REAL  TINE  TRACKING  USING  MICROPROCESSORS 
AND  A  NEW  LANGUAGE  FOR  HILITARV  SOFTUARE(U)  NAVAL 
POSTGRADUATE  SCHOOL  NONTEREV  CA  J  A  FERREIRA  27  HAR  86 

F7G  9/2  NL 


1/3 


5555 


CufttfY  CLASSlPli 


la.  REPORT  SECURITY  CLASSIFICATION 

UNCLASSIFIED _ 

2a.  SECURITY  CLASSIFICATION  AUTHORITY 


REPORT  DOCUMENTATION  PAGE 

I  lb.  RESTRICTIVE  MARKINGS 


|  2b.  OECLASSIFICATION  /  DOWNGRADING  SCHEOULE 

I 

I  4  PERFORMING  ORGANIZATION  REPORT  NUMBER(S) 


3  DISTRIBUTION  /AVAILABILITY  OF  REPORT 

Approved  for  public  release; 
distribution  is  unlimited. 

S.  MONITORING  ORGANIZATION  REPORT  NUMBER(S) 


64.  NAME  OF  PERFORMING  ORGANIZATION  6b.  OFFICE  SYMBOL  7a.  NAME  OF  MONITORING  ORGANIZATION 

(If  applicable) 

Naval  Postgraduage  School  62  Naval  Postgraduate  School 

6c.  ADDRESS  (Gty,  State,  and  ZIP  Code)  7b.  ADDRESS  (City,  State.  and  ZIP  Code) 


Monterey,  California  93943-5000 


8a  NAME  OF  FUNDING  /SPONSORING 
ORGANIZATION 


Monterey,  California  93943-5000 


18b.  OFFICE  SYMBOL  9.  PROCUREMENT  INSTRUMENT  IDENTIFICATION  NUMBER 
(If  applicable) 


8c  ADORESS  (City,  State,  and  ZIP  Cod*) 


10.  SOURCE  OF  FUNDING  NUMBERS 


PROGRAM 
ELEMENT  NO. 


PROJECT 

NO 


I  WORK  UNIT 
ACCESSION  NO. 


’  I  TITLE  (Include  Security  Classification) 

KALMAN  FILTER  REAL  TIME  TRACKING  USING  MICROPROCESSORS  AND  A  NEW  LANGUAGE  FOR  MILITARY 
SOFTWARE _ 

12  PERSONAL  AUTHOR(S) 

Jose  Alberto  Fernandes  Ferreira _ ' _ • _ 


hesis  nFbR™E  coveredto 

16  SUPPLEMENTARY  NOTATION 


14  DATE  OF^^T^jr^n^ay,  ,5  ^E  COUNT 


17 

COSATI  CODES 

FIELD 

GROUP  SUB-GROUP 

j 

18.  SUBJECT  TERMS  (Continue  on  reverse  if  necessary  and  identify  by  block  number) 

V  \ 

Kalman  Filtering;  Torpedo  Tracking  , 


'9  ABSTRACT  ( Continue  on  reverse  if  necessary  and  identify  by  block  number) 

i  A  sequential  Extended  Kalman  filter  routine,  implemented  in  the  "C" 
language,  is  developed  for  an  embedded  computer  application.  The  design 
takes  into  account  the  real  time  constraints  imposed  by  the  sampling  time 
of  the  data.  Filter  measurements  consist  of  four  (4)  transit  times  gen¬ 
erated  by  torpedo  instrumentation  in  a  TEST  RANGE  facility,  and  they  are 
nonlinear  functions  of  the  coordinates  of  the  torpedo.  Memory,  speed 
efficiency,  and  portability  are  emphasized  in  the  software  design. 

The  program  was  tested,  using  simulated  trajectories,  in  an  IBM-PC 
and  in  a  CIE  680/30  microcomputers.  Limitations  in  handling  small  floating 

zo  distribution  /  availability  of  abstract  21.  abstract  security  classification 

5P unclassified/unlimited  □  same  as  rpt  Optic  users  Unclassified 

22a  NAME  OF  RESPONSIBLE  INDIVIDUAL  22b  TELEPHONE  (Include  Area  Code)  2 2c.  OFFICE  SYMBOL 

H-  A-  Titus _ boa.  _ _ £2Ts _ 

OO  FORM  1473, 84  MAR  83  APR  edition  may  be  used  until  exhausted  SECURITY  CLASSIFICATION  OF  'his  page 

All  other  editions  are  obsolete  UNCLASSIFIED 


1 


StCuniTv  CL  »SSiriC  ATIOH  or  THIS  *»0«  r*Haa  Data  bian4 


19.  ABSTRACT  (cont'd) 

point  numbers  degraded  the  performance  of  the  algorithm  in  the 
CIE  machine  which  could  not  track  1/4-G  maneuvers.  ^ 


a 

% 


Aceoaaion  7or 
NT I 3  GTWl 
DTTC  T  A 
Uunr;.vo'...."t';’ 


□ 
n 


: 


Knffd) 


security  classification  or  this  NAcerwfc#*  o< 


Approved  -for  public  raleasa)  distribution  is  unlimited 


Kalman  Filtsr  Real  Tims  Tracking  Using 
Microprocsssors  and  a  Nsw  Languags 
for  Military  Softwara. 


by 


Joss  Albsrto  Fernandas  Farraira 
Captain,  Brazilian  Air  Fores 
B.S. ,  Institute  Tacnologico  da  Aaronautica,  1977 


Submitted  in  partial  fulfillment  of  the 
requirements  for  the  degree (s)  of 


MASTER  OF  SCIENCE  IN  ELECTRICAL  ENGINEERING 

and 

ELECTRICAL  ENGINEER 


from  the 

NAVAL  POSTGRADUATE  SCHOOL 
March,  1986 


/L  » 

Alex  Gerba,  Second^/ Reader 


73T\. 


H.B.  Ril 
Electrical 


Chairman,  Depak£ment  of 
and  Computer  Engineering' 


_ Jt  r7W _ 

J.N./Dy eri  Dean  of  Science 
and  Engineering 


3 


ABSTRACT 


A  sequential  Extended  Kalman  filter  routina,  implamantad 
in  tha  "C"  languaga,  is  davalopad  for  an  ambaddad  computar 
application.  Tha  dasign  takas  into  account  tha  real  time 
constraints  imposed  by  tha  sampling  time  of  tha  data.  Filter 
measurements  consist  of  four  (4)  transit  times  generated  by 
torpedo  instrumentation  in  a  TEST  RANQE  facility,  and  they 
are  nonlinear  functions  of  tha  coordinates  of  the  torpedo. 
Memory,  spaed  efficiency,  and  portability  are  emphasized  in 
the  software  design. 

The  program  was  tested,  using  simulated  trajectories,  in 
an  IBM— PC  and  in  a  CIE  680/30  microcomputers.  Limitations  in 
handling  small  floating  point  numbers  degraded  the 
performance  of  the  algorithm  in  the  CIE  machine  which  could 
not  track  1/4-0  maneuvers. 
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I.  INTRODUCTION 


The  problems  o-f  torpado  tracking  hava  baan  studied,  at 
tha  Naval  Postgraduate  School ,  by  aavaral  researchers,  who 
hava  implemented  simulations  that  would  allow  analysis  o-f 
tracking  algorithm  performance  using  the  Extended  Kalman 
Filter  Cll.  However,  those  studies  have  been  limited  to 
simulations  done  on  large  main-frame  computers,  without 
trying  to  adapt  them  to  the  real  time  processing  system  at 
the  torpedo  range. 

The  objective  here  is  to  develop  a  version  o-f  the 
Extended  Kalman  Filter,  adapted  to  the  constraints  o-f 
real-time  processing,  by  generating  a  program  in  the  C 
language.  Also  it  is  designed  to  run  in  the  microcomputer 
C. I.E  680/30,  currently  utilized  at  the  Naval  Torpedo 
Station,  Washington. 


The  presentation  herein  tries  to  -follow  as  much  as 
possible  the  chronological  order,  describing  the  problems  in 
the  order  o-f  appearance  and  the  way  they  were  solved. 


A.  RANGE  TRACKING  GEOMETRY 


Sets  of  hydrophona  arrays  ara  distributad  in  lina  along 
tha  axtansion  of  tha  tast  ranga.  Each  individual  array 
consists  of  four  indapandant  alamants,  placad  at  a  distanca 
"d"  apart,  on  tha  var ticas  of  a  cubs,  thus  dafining  an 
orthogonal  coordinata  systam  in  which  tha  measurement  of  tha 
transit  timas  ara  mada.  Figura  1,  shows  tha  geometry. 

Four  transit  timas,  T£ ,  T^ ,  T^,  T^ ,  corrasponding  to  tha 

four  hydrophonas  in  tha  array  ara  measured  and  the 
positional  information  is  computed  from  tha  maasurad  transit 
timas.  Tha  torpados  ara  equipped  with  sonar  transmitters 
which  ara  synchronized  with  a  master  clock,  allowing  naw 
data  every  1.31  seconds,  with  a  ranga  accuracy  between  3  to 
30  feat. 

When  multiple  arrays  are  used,  the  signal  from  the 
closest  hydrophone  is  defined  as  tha  basis  for  the  time 
measurements  and  for  the  ranga  calculations.  If  the  computer 
time  were  not  critical,  an  18  hydrophone  signal  should  be 
processed. 


Figure  li  Geometry  of  a  tracking  array 


B.  GENERAL  EXTENDED  KALMAN  FILTER  EQUATIONS 


A  summary  of  ths  filter  and  associated  aquations  is 
givan  in  this  section,  following  tha  ordar  and  tha 
con vant ions  in  Cll. 

1.  Obsarvation  Equations 

For  tha  thraa-dimansional  problem,  in  which  the 

z-position  is  considered  invariant,  a  state  spaca  consisting 

of  tha  three  positions,  X,  V  and  Z,  and  the  two  velocities, 

V  and  V  define  the  torpedo  motion.  Thus,  tha  discrete 

x  y 

linear  estimation  and  the  nonlinear  observation  equations 
are  givan  by 

X  <k+l )  -  $  .  X (k)  +  p  .  W (k)  (2.1) 

Z  <k)  -  h  <X  <k)  ,k)  +  V  <k)  (2.2) 

wharai  $  and  p  are  constant  matrices; 

h  is  a  nonlinear  function  of  the  state  X; 

W(k)  is  the  plant  excitation  noise; 

V(k)  is  the  measurement  noise. 

The  plant  noise  and  the  measurement  noise  are 
assumed  Gaussian  distributed  with  zero  mean,  and 
uncorrel ated. 

The  last  assumption  implies  that 
ECW<k) .WT< j) 3  =  Q' <k)  &kj 

ECV (k ) . VT ( j ) 3  -  R (k)  skj 
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9.  Kalman  Filter  Equation* 


P(k+l/k)-f.P(k/k) . fT+  Q(k)  (2.4a) 

G (k) «P( k/k-1 ) HT (k> CH(k)P (k/k-1 )HT(k)+R(k)  3-1  <2. 4b) 

P(k)-CI.-G(k)  .H(k)  3.P(k/k-l>  (2.4c) 

X(k+l/k)«5.X(k/k)  (2.4d) 

Z <k/k-l)«h(X(k/k-l> ,k>  <2.4e) 

X (k/k) -X (k/k-1 )+G(k) CZ(k)-Z  (k/k-1) 3  (2.4f > 


C.  APPLIED  EQUATIONS  Cll 

In  order  to  apply  the  Extended  Kalman  filter  to  the 
torpedo  tracking  problem  it  is  necessary  to  define  the  state 
vector  and  the  matrices  H,  £,  p,  W,  Q,  and  R. 

1 .  Transit  Times 

Vk)”vfcC <x+d/2) 2  +  (y+^/2>  2  +  (z+d/2) 2  Z1 /s  +v (k)  (2. 5) 

Tx  (k)**V^L^  (x~d/2)  2  + (y+d/2)  2  +  (z-*-d/2) 2  a*  /a  +v  (k)  (2.6) 

Ty  (k>“vELC  (x+d/2)2  +  (y”d/2)2  +  (z+d/2)2  I*  /2+v(k>  (2.7) 

Tz  (k>=,vlLi:  <x+d/2>2  +  <y+d/2)2  +  (z-d/2)2  //2+v(k)  (2.8) 

where:  d»30  feet,  is  the  distance  between  two  hydrophones 

in  a  given  array; 


VEL=4860  ft /sec,  is  the  average  sound  speed  at  Dabob 
Bay. 

These  equations  must  be  linearized  to  apply  Kalman 


Filter  Theory.  The  linearization  is  dans,  in  this  csss, 
about  tha  pradictad  trajectory,  which  is  being  ganarated  by 
tha  filter  algorithm. 


Tha  valuas  o-f  transit  timas  ara  corruptad  by  noise, 
v(k>,  which  is  assumad  Gaussian  with  zaro  maan.  Tha 
positions  xy  y,  and  z  ara  all  functions  of  tha  tima 
intarval . 

2.  Stata  Equations 

Tha  plant  stata  aquations  are  defined  by 


•  x  <k+l) 

*  x  <k>+TVx  <k)+gl  * 

<k+l ) 

V  <  k ) +g„ 

y  <k+l > 

- 

y (k)+TVy<k>+gJ 

Vy(k+1) 

V  ( k ) +g„ 
y  H 

.  2 (k+1)  . 

.  z  <k)+gs 

wharei  x <k> ,  y(k)  and  z <k)  ara  tha  position  coordinates  at 
time  t (k) ; 

V  (k)  and  V  <k)  are  tha  x  and  y  components  of  the 
x  y 

velocity | 

tha  depth  z<k)  is  maintained  constant  and  any 
velocity  in  z  is  assumed  random,  Gaussian, 
uncorrelated  and  with  zero  meany 
T  is  the  sampling  interval  T«1.31secy 
g  ,  g„ ,  g,  and  g„  are  excitation  terms,  included  to 

3 1  *2  3  4 

take  into  account  random  variations  in  speed  <Yy  > , 
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heading  <y  ),  and  dapth  <y  >  ,  which  ara  aaaumad  to 


ba  lndapandant,  zaro  maan ,  rataa  o-f  changa. 
Typical  torpado  manauvaring  paramatars  ara 


«K  -  22* /sac | 


EC*\.2  3 

Vt  Vt 


5-.  “  36  -ft/sac*  | 
vt 


*& 


ECy  *3 
t 


•j  ■  1  -ft/sacj 

In  stata  -form  i 
X <k+l)  -  j.X(k> 
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ECyJa 


r.W(k) 


1  T  0  0 

0  10  0 
0  0  1  T 

0  0  0  1 

L  0  0  0  0 


0 

o 

0 

0 

1 


T*/2 

T 


0 

0 


0 

0 


o 

0  T 

0  0 


Ta/2 


0 

0 

T 


(w«** +  W 


(2.  10) 


(2.11) 


(2. 12a) 
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(2.12b) 


•{•*  *  <  VV  ’«[  -  V  *»’ 


"  V«VyC,»t/Vt’",>] 

Q  is  a  symmetric  matrix  given  by 


(2. 12c) 


qO 

qi 

q2 

q3 

q4  ‘ 

qS 

q6 

q7 

q8 

q9 

qlO 

qll 

qi2 

ql3 

ql4 

(2.  13) 

qlS 

ql6 

ql7 

ql8 

ql9 

q20 

q21 

q22 

q23 

q24. 

/2)  « 

qi 

=  (T3 

/2)  9. 
> 

r« 

q2=(Ta /2)a, 

q3«<T3/2)  «. .  i  q4«0| 


q6-T2  ; 


q7»(T3/2)  «r.  i  q8*T2  q9«0; 

ql2-(Ta/2)a  *^a»  ql3»(T3/2)  »  ql4*0| 

ql8-Ta«.a;  ql9=0j  q24=Ta  *£2  ; 


ql8-Ta 


Z  <k)  -  CT  <k)  T  (k)  T  <k>  T  <k)3 
—  c  x  y  z 


0  0 


R  (k) 
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Typically  -« 

c  x 


hO 

hi 

h3 

h6 

hlO 

hll 

hl5 

hl6 

h20 

h21 

whsrsi  hO-<x * (k)+d/2) /DENI) 
h2-(y' (k>+d/2)/DENl| 
h4-(z *  <k) +d/2) /DENI | 
h6-0| 
h0-O| 

hlO-(x ' <k)+d/2)/DEN3j 
hl2-(y' <k)-d/2)/DEN3» 
hl4-<z' <k)+d/2)/DEN3| 
hi 6-0 | 
hl8-0| 

and  DENl—t  <x ' (k) +d/2) 2  +  <y 

DEN2-C <x ' <k> -d/2) 2  +  <y 
DEN3-C  <x ' (k) +d/2) 2  +  <y 


V 

-  10 
z 

sac 

h2 

h3 

h4  * 

h7 

hS 

h9 

hl2 

hl3 

hl4 

hl7 

hlB 

H19 

h22 

h23 

h24. 

hl-0| 

h3-0| 

h3-(x ' (k)-d/2) /DEN2; 
h7-(y' <k) +d/2) /DEN2j 
h9-<z ' (k) +d/2) /DEN2; 
hll-Oj 
hl3»0j 

hl5-(x ' <k)+d/2)/DEN4* 
hl7-(y' (k>  +d/2> /DEN4| 
hl9-<z ' (k)-d/2)/DEN4| 

<k) +d/2) 2 * (z ' <k)+d/2)a  3 

<k)+d/2)a  +  (z ' <k)+d/2)a  3 

<k) -d/2) 2  +  (z ' (k)+d/2)a  3 


DEN4-C  <x ' <k) +d/2> 2  +  <y ' <k)+d/2)a  +  (z ' (k) -d/2) 2  3 


D.  THE  SEQUENTIAL  EXTENDED  KALMAN  FILTER  APPROACH  C 1 3 


This  approach  assumes  that  all  -four  transit  times 
measurements  are  statistically  independent  o-f  each  other.  In 
this  way  one  can  process  the  transit  times  sequentially  and 
independently.  The  result  o-f  each  computation  step  is  used 
immediately,  as  soon  as  it  is  obtained.  Thus  more  accurate 
estimates  o-f  the  -filter  states  are  obtained. 

This  procedure  also  circumvents  the  matrix  inversion 
when  determining  the  -filter  gain,  and  allows  one  to 
disregard  any  particular  time  measurement  that  was  -found 
invalid,  and  not  the  total  -four  times  -for  a  time  slot 
<T»1 . 31sec) . 

Erroneous  transit  times  are  detected  by  implementing  a 
three  sigma  gate  using  the  covariance  o-f  the  measurement 
noise  (R>  and  the  covariance  o-f  the  estimation  error 
(P(k/k).  Far  i=l  to  4  , 

gate(i)  ■  3* < -/(P  .  .maximum) /4860  +  R.  .  )  . 

jj  ii 

It  can  also  be  used  to  minimize  position  errors  during 
high  dynamic  maneuvers,  by  repeating  the  calculations  for 
the  same  time  interval  until  the  transit  time  residual  error 


III.  THE  IMPLEMENTATION  PROBLEMS 


A.  THE  HARDWARE  AND  THE  SOFTWARE  PROBLEMS 

Sine*  the  objective  is  to  us*  ths  davsloped  software  -for 
torpedo  tracking  at  the  NAVTORPSTA,  in  Keyport,  Washington, 
there  was  no  other  choice  but  to  implement  the  program  to 
run  on  the  C.I.E.  680/30  microcomputer ,  and  to  implement  it 
in  the  C  language. 

Two  problems  immediately  arose. 

First,  there  was  only  badly  written  and  incomplete 
documentation.  The  manual  was  -found  difficult  to  read  and  to 
understand.  In  addition,  there  was  no  information  about  the 
computer  hardware. 

Secondly,  poor  software  was  available.  The  CIE  680/30 
is  a  Unix  based  machine,  designed  for  business  appl ications, 
working  with  the  Regulus  operating  system.  As  far  as  known, 
there  is  no  software  available  (specifically  written  for  the 
CIE)  on  the  market. 

As  a  result  of  these  problems,  there  was  no  full-screen 
editor  available  for  the  C.I.E  machine,  no  communications 
program,  and  no  built  in  graphics  capability  (although  the 
first  screen  that  appears  after  booting  the  system  shows  the 


message  "CIE  GRAPHICS  SYSTEM  #7"). 


B.  THE  FAST  APPROACH 


In  order  to  be  abla  to  write  a  reasonably  long  and 
complex  program,  in  a  raaaonabla  time,  debug  it  and  make  it 
work,  it  would  ba  necessary  to  have  a  good  compiler,  a  good 
■full  screen  editor  and  good  documentation  on  software  and 
hardware. 

It  was  then  decided  to  make  use  of  the  known  software 
and  hardware  information  available  for  the  IBM-PC 
microcomputer .  It  was  not  difficult  to  find  Public  Domain 
Programs  able  to  communicate  with  Unix  based  machines,  to 
emulate  the  VT-100  terminals,  and  to  transfer  files.  Also  it 
was  not  difficult  to  find  a  powerful  easy  to  use  full-screen 
editor  for  the  IBM-PC. 

The  program  was  then  written,  trying  to  keep  the  C  code 
the  most  standard  possible,  using  the  Lattice  C  compiler. 

It  was  executed  first  in  the  IBM-Pc,  then  transferred 
(source  code)  to  the  C. I.E  using  a  public  domain  program 
(SIMTERM) .  It  was  then  adapted,  making  minor  changes,  and 
recompiled  on  the  C.I.E  to  generate  the  executable  code. 

C.  ODDS  AND  EVENS  OF  C 

The  C  language  is  not  very  high  level  but  it  is  a  very 
powerful  language,  allowing  bit  operations,  low  level  read 
and  write,  system  calls  and  easy  interfaces  with  machine 
language.  The  general  mathematical  expressions  can  be  kept 


the  same  way  as  in  Fortran,  but  soma  naw  oparations, 
functions  and  expressions  are  also  usad. 


Incrementing  a  variablai 

"i*i+lj''  can  be  "i++''  or  "++i",  dapanding  if  you 
first  usa  the  variable  (++i)  and  increment  it  or 
vice  versa  <i++>. 

Incrementing  i  by  ji 

Oo  loop,  tasting  the  counting  variable  after  using  iti 
"for  <i*0| iCimax | i++> j " 

Blocks  are  delimited  by  “<  >“) 

Every  program,  subprogram,  functions  or  procedures  are 
seen  as  FUNCTIONS  in  C,  including  the  main  program.  This  is 
indicated  by  the  calling  statements  as  ini 

mainO  ....  for  the  main  program  that  is  not 

passing  any  arguments) 

plot(x,y>  ....  for  a  routine  that  passes  X  and 
Y  vectors  for  plotting,  and  so  on) 

C  uses  passing  by  value  approach.  So  the  value  of  the 
variable  "x"  is  copied  in  and  used  inside  a  function.  Arrays 
are  equivalent  to  pointers  and  thus  they  can  return  new 
values  from  functions. 

There  are  automatic,  local,  global  and  external 
variables.  There  are  character,  integer,  long  integer, 
string,  floating  point  (double  and  single  precision) , 
string,  and  enumeration  data  types.  Short  integers  have 
standard  size.  Double  precision  floating  point  allows  one  to 


represent  numbers  in  the  range  +  lO-*"  in  the  IBM-PC 

(Lattice  C  compilar)  and  in  tha  ranga  1013*  in  tha  CIE. 
Thare  ara  IF  ,  IF  THEN  ELSE,  WHILE,  00  ..  WHILE,  and  CASE 
statements. 

Thara  ara  no  input  and  output  atatamanta  aa  in  Fortran 
or  other  languagaa.  Input-output  ia  dona  by  atandard 
functions,  which  muat  ba  carefully  used. 

Header  files  and  11  #include  "  statements  are  used  to 
hide  -from  the  user  definitions  of  parameters  that  are 
machine  dependent  or  contain  environment-specific 
information.  "adefine"  statements  are  used  to  define 
constants  and  macros  and  to  hide  machine  dependent  coda 
segments. 

Reference  C41  is  the  definition  manual  of  tha  language. 
The  other  references  for  tha  C  language  ware  used  primarily 
to  look  over  the  example  programs,  and  functions. 


IV.  THE  REAL-TIME  KALMAN  FILTER  PROGRAM  IN  C 


A.  THE  INITIAL  APPROACH 

The  first  ids*  was  tha  si/npls  translation  of  the  program 
described  in  reference  Ill  to  the  C  language,  without 
worrying  about  specific  problems  that  would  arise  when 
trying,  later  on,  in  a  more  realistic  approach,  to  simulate 
as  close  as  possible  the  actual  running  conditions. 

The  initial  program  had  a  structure  similar  to  the 
Fortran  program  in  Cl  3,  that  is,  a  main  program,  the  actual 
Kalman  Filter,  and  several  functions  to  calculate  the 
trajectory,  to  add,  transpose  and  multiply  matrices  and 
to  multiply  two  vectors.  All  the  floating  point  operations 
were  done  in  double  precision  and  the  matrices  were  defined 
as  two  dimensional  arrays. 

The  initial  approach  failed  in  many  aspects. 

It  did  not  have  enough  speed,  producing,  on  the  average, 
one  estimate  every  22  sec,  in  an  IBM-PC  without  math- 
coprocessor  (8087),  running  at  5  MHz.  The  same  program 
generated  a  new  estimate  every  5  sec  in  an  IBM-PC  with  an 
8087  chip.  It  did  not  have  the  main  frame  graphics 
capability,  readily  available  and,  finally,  due  to  the  way 
the  C  language  passes  parameters  to  the  subroutines,  the 
matrix  routines  worked  correctly  only  if  matrices  were 
defined  as  square  matrices. 


B.  OPTIMIZATION  OF  EXECUTION  TIME 


Although  the  C. I.E  mi crocomputer  runs  a  Motorola  68000 
cpu  at  8  MHz,  and  the  IBM-PC  runs  at  5  MHz,  executing  the 
same  program  in  both  machines  showed  a  difference  of  10X  to 
20%  in  execution  time.  In  this  way,  optimizing  the  execution 
in  the  IBM-PC  (with  8087)  would  be  a  valid  approach  since  it 
would  allow  a  safety  margin,  and  would  require  almost  the 
same  effort. 

Optimization  of  execution  time  led  to  : 

1.  Transforming  all  the  matrices  from  bidimensional  to 
uni dimensional  arrays  (or  vectors) ,  That  saved  memory  space, 
and  forced  the  access  to  the  elements  to  conform  with  the 
way  these  are  stored  (C  stores  array  elements  by  rows),  thus 
speeding  up  retrieving  when  needed. 

2.  Eliminating  functions,  actually  by  having  everything 
in  the  main  program. 

3.  Eliminating  all  the  unnecessary  operations.  That 
involved  restructuring  the  program  to  avoid  loops  when 
possible  and  reasonable,  avoiding  multiplying  by  zero, 
avoiding  adding  zeros,  and  avoiding  repeating  calculations 
when  the  result  was  known  to  be  constant. 

4.  Avoiding  double  precision  where  possible,  and  when 
improvement  of  the  execution  time  could  be  achieved  without 
compromising  the  results.  The  Lattice  C  compiler,  in  the 
IBM-PC,  is  defined  for  precision  (all  the  floating  point 
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calculations  are  done  in  double  precision).  Then,  if  single 
precision  is  desired,  you  lose  in  both,  precision  and  speed. 
The  CIE  machine  calculations  uses  the  Motorola  "Fast 
Floating  Point",  a  format  which  is  not  compatible  with  the 
IEEE  format.  The  IEEE  floating  point  values  lose  some 
precision  due  to  the  fact  that  the  calculations  use  the  fast 
floating  point  routines  and  a  translation  to  and  from  the 
FFP  format  C103.  So,  the  FFP  format  was  used,  in  the  CIE. 

5.  Writing  single  precision  routines  to  avoid  calling 
standard  mathematical  routines,  which  are  all  double 
precision,  in  C. 

6.  Reproducing  more  closely  the  way  the  final  program 
would  have  to  work,  when  in  the  real  time  application. 

7.  Testing  the  program  for  several  values  and 
parameters,  to  determine  bounds  that  would  allow  results  of 
good  precision  without  increasing  the  execution  time.  It  was 
found,  for  example,  that  the  results  are  highly  sensitive  to 
the  way  the  columns  of  the  matrix  H  are  calculated,  thus, 
requiring  calculation  of  H  using  double  precision. 

8.  Measuring  execution  time  of  tasks  inside  the 
program. 

9.  Developing  software  tools  to  allow  for  interactive 


execution  and  checking  of  correctness  of  the  results 


V.  TESTING 


A.  CHECKING  THE  TRAJECTORY-FOLLOWING  PERFORMANCE 

The  trajectory  ■following  performance  was  verified  for 
the  following  trajectories  : 

1.  Straight  line  parallel  to  the  array  axis,  and 
y-coordinate  ■  0.0  ,  single  array  tracking; 

2.  Straight  line  parallel  to  the  array  axis  and 
y-coordinate  =  2121.0  ,  single  array  tracking; 

3.  Straight  line,  45?  inclination  relatively  to  the 
array  axis,  single  array; 

4.  Straight  line  parallel  to  the  array  axis,  long  run, 
multiple  array  tracking; 

5.  Straight  line  and  circular  maneuver,  short  run,  slow 
turn  <10.0dg/sec  ,  1/4-G) ,  single  array  tracking; 

b.  Straight  line  and  circular  maneuver,  long  run,  slow 
turn  (lO.Odg/sec  ,  1/4-G),  multiple  array  tracking; 

7.  Straight  line  and  circular  maneuver,  short  run,  fast 

turn  (20dg/sec  ,  1-G) ,  single  array  tracking,  adaptive 

maneuver  implemented  with  4  iterations  allowed; 

8.  Straight  line  and  circular  maneuver,  long  run,  fast 
turn  <20  dg/sec  ,  1-G),  multiple  array  tracking,  adaptive 


maneuver  implemented  with  4  iterations  allowed 


TABLE  1  ,  and  TABLE  2  summarize  the  r vault a  in  tarma  of 
maximum  absolute  error  for  the  IBM  and  the  CIE  microcom¬ 


puters,  respectively.  For  the  single  array  cases,  only  the 
region  inside  a  3000  feet  radius  around  the  center  of  the 
array  being  used  is  considered  in  determining  the  errors. 

The  corresponding  plots  can  be  found  in  the  following 

— 1  o 

pages.  The  plots  were  obtained  with  the  value  10  for  the 
elements  in  the  diagonal  of  the  matrix  R  (the  Error 
Covariance  Matrix)  for  the  IBM-PC  microcomputer  and  with  the 

value  10  '  for  the  CIE.  The  plots  are  numbered  sequentially 
and  contain  headings  to  identify  and  relate  them  to  the 
Table  1  and  Table  2.  The  actual  time  is  obtained  by 
multiplying  the  time  scale  by  the  length  of  the  time  slot, 
1  i  31  seconds. 

B.  CHECKING  THE  EXECUTION  TIME  REQUIREMENT 

The  execution  time  in  the  IBM-PC,  was  verified  by 
checking  the  printouts  containing  the  values  of  the  internal 
computer  time  for  a  cycle.  Later,  an  “waiting  loop"  was 
introduced  to  generate  an  interval  of  1.31  sec  between  each 
data  input  (that  is,  reading  files  TRJ.DAT  and  ZIC.DAT). 

Since  the  "reading  internal  time"  was  not  incorporated 
in  the  CIE  program  version  (CIEKR1),  and  since  that  proved 
to  be  a  faster  machine,  the  time  requirement  was  just 
checked  against  a  chronometer  for  150  iterations. 
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RUN  I  MAX.  ABS.  PCS.  ERROR  1  TRAJEC  I  ARRAY  ,*  INITIAL  i 

i  -  I  I  I  I 

#  i  X  ERR  I  Y_ERR  i  Z_ERR  I  TYPE  i  TYPE  I  CONDIT.  ! 


1 inter 


1 invar  I  singl 


1  i  mar 


circl 
lOdg/s 
t0  =40 
tf =70 


circl 
20dg/s 
t0  =35 
tf  =45 


x  -3000 
y  =  0 
Vx  —  50 
Vy-  0 

x  -3000 
y  -2121 
Vx— 33.8 
Vy-  0 

x  -3000 
y  —3000 
Vx— 33.  8 
Vy-  33.8 

x  -34000 
y  -6050 
Vx— B4.4 
Vy-  0 

x  -3000 

y  -  0 

Vx— 50 
Vy-  5 


17 

!  3. 

3 

2.8 

circl 

lOdg/s 

mul  tp 

x  =10000 
y  =  0 

t0  =40 
tf  =70 

Vx— 50 
Vy=  5 

circl 
20dg/s 
t0 =120 
tf =130 


20000 


8 


2 


2 


TABLE  2 


l 


CIE  MICROCOMPUTER 


RUN  !  MAX.  ABS.  POS.  ERROR  !  TRAJEC  !  ARRAY  I  INITIAL 

I  -  I  I  j 

#  i  X  ERR  I  Y  ERR  I  Z  ERR  !  TYPE  I  TYPE  I  CONDIT. 

- . - 1 - 1 - S - 1 - j - 

I  I  I  t  I  lx  -3000  I 


i  :  0.031 

i 

i 

4 

1  1  1 invar  I  singl  i  y  -  0 

1  1  1  Vx— 50 

1  1  1  Vy=  0 

III 

i 

2  1  7.1 

1 

1 

1 

1 

■ 

10 

1 

1 

1.2  I  1 invar 

1 

1 

1 

( 

1 

|  x  =3000 
aingl  i  y  =2121 

1  Vx— 33.8 

1  Vy=  0 

| 

3  1  3 

1 

1 

1 

1 

3 

1 

0.9  1  1 invar 

1 

t 

1 

1 

1 

i  x  =3000 
singl  1  y  —3000 

1  Vx— 33.8 

1  Vy»  33.8. 

4 

0.2 

1 

1 

3.5  1  0.7 

1 

I 

1 

1 invar 

mul  tp 

x  =34000 
y  =6050 
Vx— 84.4 
Vy=  0 

5 

circl 
lOdg/s 
t0  =40 
tf  =70 

singl 

x  =3000 

y  =  0 

Vx=-50 
Vy=  5 

6 

- — 

— 

circl 
lOdg/s 
t0  =40 
t-f  =70 

mul  tp 

x  =10000 
y  =  0 
Vx=-50 
Vy=  5 

7 

— 

circl 
20dg/s 
t0  =35 
t-f  =45 

singl 

x  =5000 
y  =  0 
Vx=-92. 5 
Vy*  0 

circl 
20dg/s 
t0 =120 


x  =20000 
y  =  0 
Vx=-92. 5 


t-f  =  130 


Vy=  5 
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mul  tp 


MU. 


Figure  S.  IBM-PC  -  Error  in  torpedo  position  during  a 
straight  run  in  the  area  o-f  a  single  array.  Initial 
conditionsi  x*3000  ftf  y«0  -ft,  Vx*-50  -f t/sec,  Vy«0  ft/sec. 


Figure  7.  IBM-PC  -  Variance  of  position  error  during  a 
straight  run  in  the  area  of  a  single  array.  Initial 
conditions!  x-3000  ft,  y»0  ft,  Vx— 50  ft/sec,  Vy-0  ft/sec. 


Figure  8.  IBM-PC  -  Variance  o-f  position  error  during  a 
straight  run  in  the  area  o-f  a  single  array.  Initial 
conditionsi  x«3000  -ft,  y»0  ft,  Vx»-30  -ft/sec,  Vy-0  ft/sec. 
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conditions!  x*3000  -ft,  y«*0  -ft.  Vx—50  -ft/sec,  Vy-0  ft/sec. 
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Figure  10.  IBM-PC  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  o-f  a  single  array.  Initial 
conditions!  x»3000  ft.  y-0  ft,  Vx  —  50  ft/sec,  Vy-0  ft/sec. 


Figure  11.  IBM-PC  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditionsi  x-3000  ft,  y-0  ft,  Vx— 50  ft/sec,  Vy-0  ft/sec. 


Figure  12.  IBM-PC 
during  a  straight  run 
conditionsi  x*3000  •ft. 


—  Filter  estimate  of  mean— square  error 
in  the  area  of  a  single  array.  Initial 
y-0  ft,  Vx»-50  ft/sec,  Vy-0  ft/sec. 
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Figure  13.  IBM-PC  -  Real  time  plot  of  estimated  trajectory 
of  the  torpedo  in  the  area  of  a  single  array.  Initial 
conditionsi  x“3000  ft.  ym212l  ft*  Vx*--33.8  ft/s*  Vy*0  ft/s. 
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Figure  15.  1BH-PC  -  Estimate  of  torpedo  position  during  a 
straight  run  in  the  area  of  single  array.  Initial  conditions 
are i  x-3000  ft,  y-2121  ft,  Vx— 33.8  ft/s,  Vy-0  ft/s. 


Figure  17.  IBM-PC  -  Error  in  torpedo  position  during  a 
straight  run  in  the  area  of  a  single  array.  Initial 
conditionsi  x-3000  ft,  y-2121  ft,  Vx— 33.8  ft/s,  Vy-0  ft/s. 


Figure  19.  IBM-PC  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditions!  x-3000  ft,  y-2121  ft,  Vx»-33.B  ft/s,  Vy-0  ft/s. 
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Figure  20.  IBM-PC  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditions!  x«3000  ft,  y-2121  ft,  Vx  —  33. B  ft/s,  Vy»0  ft/s. 
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Figure  21.  IBM-PC  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  o-f  a  single  array.  Initial 
conditions!  x-3000  -ft.  y-2121  ft,  Vx— 33.8  ft/s,  Vy-0  ft/s. 


Figure  22.  IBM-PC  -  Real  time  plot  of  estimated  trajectory 
of  the  torpedo  in  the  area  of  a  single  array.  Initial  condi¬ 
tions!  x-3000  ft,  y— 3000  ft,  Vx  —  33.8  ft/s,  Vy— 33.  B  ft/s. 
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Figure  24.  IBM-PC  -  Estimate  of  torpedo  position  during  a 
straight  run  in  the  area  of  single  array.  Initial  conditions 
are*  x-3000  ft,  y— 3000  ft,  Vx— 33.8  ft/s,  Vy-33.8  ft/s. 


Figure  25.  IBM-PC  -  Error  in  torpedo  position  during  a 
straight  run  in  the  area  of  a  single  array.  Initial  con¬ 
ditions*  x-3000  ft,  y»-3000  ft,  Vx=-33.8  ft/s,  Vy-33.8  ft/s. 
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Figure  26.  IBM-PC  -  Error  in  torpedo  position  during  a 
straight  run  in  the  area  of  a  single  array.  Initial  con¬ 
ditions*  x-3000  ft,  y»-3000  ft,  Vx  —  33.8  ft/s,  Vy-33.B  ft/s. 


Figure  27.  IBM-PC  -  Error  in  torpedo  depth  during  a 
straight  run  in  the  area  of  single  array.  Initial  conditions 
i  x-3000  ft,  y— 3000  ft,  Vx— 33.8  ft/s,  Vy-33.8  ft/s. 


Figure  28.  IBM— PC  —  Filter  estimate  of  mean— square  error 
for  a  straight  run  in  the  area  of  single  array.  Initial  con¬ 
ditions!  x«3000  ft,  y«-3000  ft,  Vx  —  33.8  ft/s,  Vy-33.8  ft/s. 
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Figure  29.  IBM-PC  -  Filter  estimate  of  mean-square  error 
for  a  straight  run  in  the  area  of  single  array.  Initial  con¬ 
ditions!  x«3000  ft,  y»-3000  ft.  Vx“-33.B  ft/s,  Vy»33.B  ft/s. 


Figure  31.  IBM-PC  -  T rue  trajectory  of  the  torpedo  in  the 
during  a  straight  runthrough  multiple  arrays.  Initial  condi¬ 
tions!  m«34000  ft,  y«6030  ft,  Vx— B4.4  ft/sec,  Vy«0  ft/sec. 
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Figure  32.  IBM-PC  -  Estimate  of 
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Figure  33.  IBIi-PC  -  Error  in  torpedo  position  during  a 
straight  run  through  multiple  arrays.  Initial  conditions 
are*  **34000  -ft,  y*6050  -ft,  Vx*-84.4  ft/s,  Vy*0  -ft/s. 
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Figure  36.  IBM-PC  -  Filter  estimate  of  mean-square  error 
for  a  straight  run  through  multiple  arrays.  Initial  condi¬ 
tions!  **34000  ft,  y*6050  ft,  Vx«-S4.4  ft/s,  Vy»0  ft/s. 


Figure  37.  IBM— PC  —  Filter  estimate  o-f  mean— square  error 
•for  a  straight  run  through  multiple  arrays.  Initial  condi¬ 


tions!  x *34000  -ft,  y*6050  -ft,  Vx—84.4  -ft/s,  Vy»0  -ft/s. 
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Figure  40.  IBM-PC  -  True  trajectory  of  the  torpedo  during  a 
10  dg/s  maneuver  in  the  area  of  a  single  array.  Initial 
canditionsi  x*3000  ft,  y-0  ft,  Vx«-50  ft/sec,  Vy«5  ft/sec. 


Figure  41.  IBM-PC  -  Estimate  of  torpedo  position  during  a 
10  dg/s  maneuver  in  the  area  of  a  single  array.  Initial 
conditionsi  x«3000  ft,  y»0  ft,  Vx«-50  ft/sec,  Vy«5  ft/sec. 


Figur*  43.  IBM-PC  -  Error  in  torpedo  position  during  a  10 
dg/s  maneuver  in  the  area  o-f  a  single  array.  Initial 
conditionsi  x*3000  ft,  y«0  ft,  Vx«-50  ft/sec,  Vy»3  ft/sec. 
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Figura  44.  IBM-PC  -  Error  in  torpedo  depth  during  a  10  da/s 

ln  ih*  ar“*  of  *  single  array.  Initial  conditions* 
x-3000  ft,  y-0  ft,  Vx  —  50  ft/sec,  Vy-S  ft/sec. 


Figure  46.  IBM-PC  -  Estimate  of  torpedo  position  during  a 
10  dg/s  maneuver  thruough  multiple  arrays.  Initial 
conditions!  x-1000  ft,  y-0  ft,  Vx—50  ft/sec,  Vy-5  ft/sec. 


Figure  47.  IBM-PC  -  Error  in  torpedo  position  during  a  10 
dg/s  maneuver  through  multiple  arrays.  Initial  conditions! 
k-1000  -ft.  y-0  it ,  Vx— 50  ft/sec.  Vy-5  i t/sec. 


Figure  48.  IBM-PC  -  Error  in  torpedo  position  during  a  10 
dg/s  maneuver  through  multiple  arrays.  Initial  conditions* 
><■1000  ft.  y«0  ft,  Vx— 50  ft/sec,  Vy=»5  ft/sec. 


Figure  49.  IBM-PC  -  Error  in  torpedo  position  during  a  10 
dg/s  maneuver  through  multiple  arrays.  Initial  conditions: 
x»1000  -ft,  y»0  ft,  Vx«-50  ft/sec,  Vy«5  ft/sec. 


Figure  50.  IBM-PC  -  True  trajectory  of  the  torpedo  during  a 
20  dg/s  maneuver  in  the  area  of  a  single  array.  Initial  con¬ 
ditional  x-5000  ft,  y«0  ft,  Vx— 92.5  ft/sec,  Vy-0  ft/sec. 


Figure  31.  IBM-PC  -  Estimate  of  torpedo  position  during  a 
20  dg/s  maneuver  in  the  area  of  a  single  array.  Initial 
conditionsi  x*5000  ft,  y“0  ft,  Vx*-92.5  ft/sec,  Vy»0  ft/sec. 
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Figure  32.  IBM-PC  -  Error  in  torpedo  position  during  a  20 
dg/s  maneuver  in  the  area  of  a  single  array.  Initial 
conditions!  x-5000  ■ft,  y«0  -ft,  Vx«-92.5  ft /sec,  Vy»0  ft/sec. 
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Figure  53.  IBM-PC  -  Error  in  torpedo  position  during  a  20 
dg/s  maneuver  in  the  area  of  a  single  array.  Initial 
conditional  x»5000  -ft,  y»0  -ft,  Vx— 92.5  -ft/sec,  Vy=0  -ft/sec. 


Figure  54.  IBM-PC  -  Error  in  torpedo  depth  during  a  20  dg/s 
maneuver  in  the  area  of  a  single  array.  Initial  conditions: 
x-5000  -ft,  y=0  ft,  Vx=-92. 5  ft/sec,  Vy=0  ft/sec. 


Figure  55.  IBM- PC  —  Real  time  plot  of  estimated  trajectory 
of  the  torpedo  through  multiple  arrays.  Initial  condi¬ 
tions*  x *20000  ft,  y=0  ft,  Vx=-92. 5  ft/sec,  Vy=0  ft/sec. 


Figure  56.  IBM-PC  -  True  trajectory  of  the  torpedo  in  the 
during  a  20  dg/s  maneuver  through  multiple  arrays.  Initial 
conditions*  x-20000  ft,  y-0  ft,  Vx»-92.5  ft/s,  Vy-0  ft/s. 


css  oa  c —  u->  'f  cvj  — «  on  r-  no  «cr 

cv,  ^  - — i  — «  ■*— i  — h  - — i  ts  co  csa  cb 


Figure  57.  IBM-PC  -  Estimate  of  torpedo  position  during  a 
20  dg/s  maneuver  through  multiple  arrays.  Initial 
conditions!  x«20000  ft,  y«0  ft,  Vx®-92.5  ft/s,  Vy»0  ft/s. 
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Figure  60.  IBM-PC  -  Error  in  torpedo  position  during  a  20 
dg/s  maneuver  through  multiple  arrays.  Initial  condi tionsi 
x "20000  -ft,  y-0  -ft,  Vx  — 92.5  it/%.  Vy-0  it/%. 
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Figure  61.  CIE  -  True  trajectory  of  the  torpedo  in  the 
area  of  a  single  array.  Initial  conditions  i  x«3000  ft,  y«0 
ft,  Vx  —  50  ft/sec,  Vy-0  ft/sec. 


Figure  64.  CIE  -  Error  in  torpedo  position  during  a 
straight  run  in  the  area  of  a  single  array.  Initial 
conditions!  x*3000  ft.  y*0  ft,  Vx»-50  ft/sec,  Vy=0  ft/sec. 
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Figure  63.  CIE  -  Error  in  torpedo  depth  dur’  g  a  straight 
run  in  the  area  of  a  single  array.  Init  *1  conditions* 
x *3000  ft,  y-0  ft,  Vx—  50  ft/sec,  Vy-0  ft/sec. 


Figure  66.  CIE  -  Variance  of  position  error  during  a 
straight  run  in  the  area  of  a  single  array.  Initial 


conditions!  x»3000  ft,  y»0  ft,  Vx*-50  ft/sec,  Vy*0  ft/sec. 


Figure  69.  CIE  ~  Filter  estimate  of  mean— square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditions*  x-3000  ft,  y-0  ft,  Vx—  50  ft/sec,  Vy-0  ft/sec. 


Figurm  71.  CIE  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditions!  x«3000  ft*  ym0  ft,  Vx"-50  ft/sec,  Vy“0  ft/sec. 
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Figure  72.  CIE  -  True  trajectory  o-f  the  torpedo  in  the 
area  o-f  a  single  array.  Initial  conditions  i  x-3000  ft, 
y-2121  ft,  Vx— 33.8  ft/sec,  Vy-0  ft/sec. 
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Figure  74.  CIE  -  Error  in  torpedo  position  during  a 
straight  run  in  the  area  of  a  single  array.  Initial 
conditions*  x-3000  -ft,  y-2121  ft,  Vx«-33.8  -ft/s,  Vy»0  ft/s. 
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Figure  76.  CIE  -  Error  in  torpedo  depth  during  a  straight 
run  in  the  area  of  a  single  array.  Initial  conditions* 
x-3000  ft,  y-2121  ft,  Vx  —  33.  S  ft/s,  Vy-0  ft/s. 


Figure  77.  CIE  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditions!  x«3000  ft.  y*2121  ft,  Vx»-33.S  ft/s,  Vy“0  ft/s. 
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Figure  79.  CIE  -  Filter  estimate  of  mean-square  error 
during  a  straight  run  in  the  area  of  a  single  array.  Initial 
conditions*  x«3000  ft,  y«2121  ft,  Vx»-33.B  ft/s,  Vy=*0  ft/s. 


Figure  81.  CIE  -  Estimate  of  torpedo  position  during  a 
straight  run  in  the  area  of  single  array.  Initial  conditions 
are*  x-3000  ft,  y— 3000  ft,  Vx— 33.8  ft/s,  Vy-33.8  ft/s. 
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Figure  82.  CIE  -  Error  in  torpedo  position  during  < 
straight  run  in  the  area  of  a  single  array.  Initial  condi¬ 
tions!  x-3000  ft,  y— 3000  ft,  *>x—  33.  B  ft/s,  Vy-33.8  ft/s. 


Figure  84.  CIE  -  Error  in  torpedo  depth  during  a  straight 
run  in  the  area  of  single  array.  Initial  conditions!  x*3000 
■ft,  y— 3000  ft,  Vx  — 33.8  ft/s,  Vy-33.8  ft/s. 


Figure  85.  CIE  -  True  trajectory  of  the  torpedo  in  the 
during  a  straight  runthrough  multiple  arrays.  Initial  condi¬ 
tions!  x«34000  ft,  y-6050  ft,  Vx«-B4.4  ft/sec,  Vy«0  ft/sec. 


Figure  86.  CIE  -  Estimate  of  torpedo  position  during  a 
straight  run  through  multiple  arrays.  Initial  conditions 
are*  x«34000  -ft,  y-6050  -ft,  Vx«-B4.4  ft/s.  Vy«0  ft/s. 
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Figure  87.  CIE  -  Error  in  torpedo  position  during  a 
straight  run  through  multiple  arrays.  Initial  conditions 
arei  x-34000  ft,  y-6050  ft,  Vx  —  84.4  ft/s,  Vy-0  ft/s. 
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Figure  88.  CIE  -  Error  in  torpedo  position  during  a 
straight  run  through  multiple  arrays.  Initial  conditions 
are*  x *34000  ft,  y*6030  ft,  Vx*-B4.4  ft/s,  Vy*0  ft/s. 


Figure  89.  CIE  -  Error  in  torpedo  depth  during  a  straight 
run  through  multiple  arrays.  Initial  conditions  arei  x-34000 
ft,  y-6050  ft,  Vx—84.4  ft/s,  Vy-0  ft/s. 
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VI.  COMMENTS  AND  CONCLUSIONS 


Thar*  are  slightly  different  varsions  of  tha  program, 
for  tha  IBM-PC  and  for  tha  CIE  mi crocomputar.  However,  the 
Saquancial  Extended  Kalman  Filtar  program  presented  have 
many  common  characteristics.  The  execution  time  per  cycle, 
in  both  machines,  is  lass  than  tha  sample  interval  for  the 
data.  The  program  is  writtan  in  tha  "C"  programming 
language,  and  it  is  maant  to  ba  reasonably  portable.  Double 
precision  calculations  are  widely  usad,  and  allowed  in  many 
cases  to  bound  errors  without  repeating  tha  calculations  for 
the  same  time  interval. 

Calculations  are  performed  using  an  adaptive  Q  matrix. 
That  is,  tha  matrix  Q  is  recalculated  each  iteration, 
instead  of  using  a  constant  value.  Tha  trajectory  simulation 
routine  was  implemented  using  geometrical  approach.  The 
tangential  speed  is  assumed  constant  during  the  whole  run. 

The  Kalman  Filter  can  track  through  single  and  multiple 
arrays.  The  algorithm  always  uses  the  coordinates  of  the 
closest  array  in  the  calculations,  for  the  multiple  array 
case.  There  is  essentially,  in  either  CIEKR1  and  KR,  only  a 
main  program  without  subroutine  calls.  The  bi dimensional 
matrices  were  transformed  and  are  used  as  vectors.  Most 
loops  were  eliminated  to  keep  the  execution  time  within  the 
requirad  limits. 
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The  CIE  version  can  also  run  in  tha  IBM-PC  but  tha  IBM- 
PC  varsion  runs  only  in  tha  IBM  sinca  it  calls  assembly 
language  routines  to  generate  graphic  output.  The  IBM 
version  is  slower  than  the  CIE  version  and  the  IBM-PC 
microcomputer  must  run  with  its  80B7  math  coprocessor  chip 
to  satisfy  the  speed  requirements.  Both  machines  were 
configured  with  hard  disks,  and  the  data  files  were  read 
from  and  written  to  the  hard  disks.  Both,  the  IBM-PC  and  the 
CIE  versions,  will  run  on  the  IBM-AT. 

There  is  almost  no  documentation  or  software  available 
for  the  CIE  microcomputer .  The  lack  of  documentation,  the 
lack  of  a  good  compiler,  a  full-screen  editor,  and  the  lack 
of  graphics  capability  were  major  problems  in  using  the  CIE. 

The  improvement  in  the  execution  time  was  more 
noticeable  in  the  CIE  than  in  the  IBM.  This  is  probably  due 
to  the  presence  of  the  math  coprocessor  chip,  8087,  and  (or) 
due  to  a  better  compiler, in  the  IBM-PC.  The  use  of  the  "~f" 
("fast  floating  point  format")  option,  when  compiling  code 
on  the  CIE,  seemed  to  help  both,  the  speed  of  execution  and 
the  precision  of  the  results. 

The  programs  were  tested  for  torpedo  speeds  from  20 
knots  (33.8  ft/s)  to  55  knots  (92.8  ft/s)  and  turn  rates  of 
up  to  20  dg/sec,  or  maneuvers  of  up  to  1-G.  The  magnitude  of 
the  position  errors  for  linear  trajectories  were  maintained 
less  than  10  ft  in  either  computer.  The  CIE  could  not  track 


any  of  the  circular  maneuvers  (run  numbers  6.  7  and  8  in 


Table  2).  In  the  IBM-PC,  keeping  the  maneuver  within  the 
tracking  radius  of  a  single  array  (  +  3000  -ft  around  the 


array  center),  the  magnitude  o-f  position  errors  were 
maintained  less  or  equal  to  7  -feet  -for  an  1-6  maneuver  and 
less  or  equal  to  22  -feet  -for  the  multiple  array  case. 

The  high  error  obtained  -for  the  circular  maneuvers  in 
either  the  single  or  multiple  array  case  re-flects  the 
algorithm  inability  to  -fallow  -fast  turn  circular 
trajectories.  In  Cl]  such  high  errors  were  reduced  to  values 
comparable  to  the  errors  -found  in  the  straight  runs,  by 
adding  the  adaptive  Q  matrix  to  PKK  to  produce  PKKM1  and 
repeating  the  calculations  for  the  same  time  interval.  This 
was  called  "adaptive  maneuver".  The  "adaptive  maneuver"  as 
implemented  (see  segment  2,  in  the  algorithm,  in  Appendix  A) 

—1  0 

was  found  effective  for  the  IBM-PC  when  the  value  10  was 
used  for  the  diagonal  elements  of  the  matrix  R.  However,  to 
keep  the  execution  time  per  cycle  within  1.31  sec,  the 
number  of  adaptive  maneuvers  performed  had  to  be  limited. 
Almost  no  improvement  in  reducing  tracking  errors  was 

achieved  when  the  value  used  was  10  *.  For  the  later  case  a 
great  deal  of  degradation  in  the  execution  time  was  observed 
and  the  time  interval  was  exceeded  even  when  just  one  more 
iteration  (optimum  maneuver)  was  executed.  The  procedure  did 
not  work  on  the  CIE,  which  could  not  handle  numbers  less 


than  10  reliabily.  The  "adaptive  maneuver"  feature  was 
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APPENDIX  A 


PROGRAM  DESCRIPTION 

A.  OVERVIEW 

There  are  at  this  moment,  thrss  versions  of  the  Extended 
Kalman  Filter  program.  In  either  case  the  simulated 
trajectory  must  be  -first  computed  and  the  hydrophones  array 
times,  corresponding  to  the  simulated  positions,  are  stored 
in  a  -file  in  order  to  be  read  in.  All  three  programs  write 
the  files  XKK.DAT,  PKK.DAT,  and  XDIFF.DAT  to  the  disk. 

To  avoid  delays  due  to  data  writing  and  retrieving  from 
disk  drives,  the  data  files  can  be  written  to  and  read  from 
an  electronic  drive  built  in  the  RAM  memory.  There  is  also  a 
utility  program  -  PLOT.C  -  which  allows  fast  graphic 
analysis  after  the  run. 

1 .  The  GIE1.C  Program 

This  was  the  first  program  to  run  in  the  CIE 
microcomputer.  It  reads  "clean"  (without  noise)  trajectory 
data  from  the  file  TRAJ.DAT  and  writes  3  columns, 
corresponding  to  the  coordinates  x,  y,  and  z,  to  the  disk. 
The  code  to  generate  Gausian  noise  is  contained  in  the  main 
program. 

CIE1.C  generates  3  data  files,  XKK.DAT,  PKK.DAT,  and 
XDIFF.DAT,  containing  filtered  data  to  be  post-processed. 
XKK.DAT,  contains  4  columns,  corresponding  to  the  time  slot 
and  the  filter  estimates,  X(k/k),  for  the  coordinates  x,  y, 


and  z  in  that  ordar.  PKK.DAT  contains  4  columns  with  ths 


correspond!  ng  valuss  of  ths  tims  slot  and  ths  srror 
covariance  matrix  -for  the  coordinates  x,  y,  and  z,  in  that 
order.  XDIFF.DAT  contains  4  columns  with  ths  corresponding 
valuss  of  ths  tims  slot  and  the  errors  for  ths  coordinates, 
x,  y,  and  z,  in  that  order. 

2.  Ths  CIEKR1.C  Program 

This  program  runs  in  ths  CIE  and  provides  input  and 
output  similar  to  ths  KR.C  .  It  is  able  to  simulate  only 
straight  line  trajectories,  due  to  the  lack  of  an  "arc  tan" 
function  in  the  ClE's  C  Compiler,  and  at  present  does  not 
access  the  computer  internal  clock. 

3.  The  KR.C  Program 

This  program  includes  a  set  of  assembly  language 
routines  for  interfacing  with  the  keyboard,  display,  and 
output  ports  of  the  IBM-PC/AT.  It  also  includes  a  plotting 
routine,  written  in  C,  which  calls  the  assembly  routines  to 
generate  the  frame,  to  print  updated  values,  and  to  draw  the 
estimated  trajectory.  Another  assembly  language  routine  was 
added  to  allow  access  to  the  internal  clock  in  the  IBM-PC. 
See  references  171  and  C83  and  Appendix  C  for  details. 

The  Kalman  Filter  section  of  KR.C  needs  the  files 
TRJ.DAT  and  ZIC.DAT,  generated  by  the  trajectory  simulation 
routine,  for  inputs. 

The  trajectory  simulation  generates  transit  times 


data  already  corrupted  with  noise.  That  involves  inclusion 


of  the  naim»  generation  rout in*  with  the  "clean"  trajectory. 
The  cl  van  trajectory  can  be  either  a  straight  line  or  a 
circular  curve. 

The  trajectory  simulation  routine  creates  the  -files 
TRJ . DAT ,  ZIC.OAT ,  TRJ. INP,  and  PAR AM. DAT. 

The  TRJ. DAT  contains  the  data  for  the  clean 
trajectory,  corresponding  to  the  time  slot,  and  the  x,  y  and 
z  coordinates.  Z1C.DAT  contains  the  noise  corrupted  transit 
times.  TRJ.INP  and  PARAM.DAT  contain  information  to  be  used 
by  PLOT.C  program  in  order  to  identify  and  relate  the 
trajectory  data  and  the  parameters  used  in  the  Kalman  Filter 
to  the  graphic  outputs. 

B.  DETAILED  DISCUSSION 

Since  the  programs  evolved  one  from  the  other,  only  the 
last  one,  KR.C,  will  be  discussed  in  more  detail.  The 
routine  that  generates  the  trajectory  and  the  tools 
developed  to  facilitate  the  objective  analysis  of  results 
will  also  be  briefly  described. 

1.  The  Kalman  Filter  Proaram  -  KR.C 


The  algorithm  for  the  program  KR.C  is  provided  as 
Appendix  B  and  the  actual  program  implementation  is  shown  in 
the  Appendix  E. 

The  capability  of  tracking  trough  multiple  arrays  is 
implemented  by  simply  defining  switching  points  along  a 
coordinate  axis,  and  comparing  the  estimated  x-position 
against  switching  values.  The  coordinates  of  the  tracking 
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array  are  than  changad  in  ordar  to  always  kaap  tha  closast 
array  tracking  tha  torpado.  Figurs  90  shows  tha  coordinata 
systam  -for  tha  multiple  array  tracking,  tha  location  of  tha 
switching  points  and  the  hydrophone  location  matrix. 

Tests  performed  using  a  straight  line  trajectory  and 
keeping  the  Q  matrix  constant  showed  no  improvement  over  the 
case  where  tha  adaptive  Q  is  used. 

2.  The  Trajectory  Simulation  Routine  -  TRAJEC 

This  routine  was  developed  as  an  independent 
simulation  program,  to  generate  trajectories,  where  circular 
and  linear  segments  can  be  combined  to  produce  a  desired 
path. 

Given  the  initial  x,y,z,  positions  and  the  initial 

speeds  and  assuming  that  the  vertical  speed  is  zero,  this 

routine  generates  noisy  data  to  represent  the  "raw"  data  as 

it  would  come  from  the  hydrophones.  TRAJEC  finds  the 

position  of  a  point  in  a  circle  after  a  time  interval  T, 

given  the  tangential  speed  of  the  point,  and  given  the 

desired  turn  rate  or  given  the  centrifugal  acceleration.  It 

is  assumed  that  the  tangential  speed  is  constant  during  the 

maneuver.  Either  the  turn  rate  or  the  centrifugal 

acceleration  can  be  used  to  determine  the  radius. 

Figure  91  represents  the  geometry  of  the  problem.  The 

straight  line  shown  is  tangent  to  circle.  A  point  is 

initially  moving  on  the  line  and  at  the  time  t„  it  is 
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sub  j  act  ad  to  centrifugal  force  which  makes  it  turn.  The 
point  then  describes  a  circular  movement  generating  a  circle 
of  radius  R,  and  at  the  time  tg  +T  will  be  at  the  position 

X y Y .  Assume  V  and  V  known  speeds ,  m  *  V  /V  is  then  the 

y  x  y  * 

slope  of  the  line  tangent  to  the  circle  that  passes  at  the 
point  xg  ,y0  and  has  center  at  a,b.  The  angular  coefficient 

o-f  the  line  perpendicular  to  the  circle  and  passing  at  the 


point  xg,yg  ii 


<xg  -  a)  /  (yq  -  b)  *  -  1  /  m  ,  so 


xo  " 


m  *  <y  -  b) 
o 


Rz  ■  (x  -  a) *  +  (y  -  b>' 


is  valid  for  any  point 


on  the  circle. 


at  *„  f  v0  ■»  r2  ■  <*0  "  a>2+  {y0~  b)2 


Rz  »  (mz  -  1)  *  <yQ  -  b)' 


y0  -  b  *  +  R  /  sqrt  (1  +  nr  ) 


or  b  *  yg  +  R  /  sqrt  (1  +  nr  ) 


and  a  »  x„  +  m  *  <y„  -b) 
0  0 


For  any  point  on  the  circle 

x  -  a  =  R  cos  d 
y  -  b  ■  R  sin  e 
From  the  Figure  91 


•  '.AV*/*.*  .V 


<*  ■  tan  m 


d0»  ff  /  2  -  ac 


©  *  ®0  +  (it  A  t 


Gi van  Vx  ,  Vy  4  Vfc  -  sqrt (V*  +  V*> 


Bi  van  <j 


4  R  -  Vt  /  tt 


Given  the  central  acceleration,  g  4  <j  *  g  / 

At  the  end  o-f  the  interval,  the  position  Mill  be  x, 
y  and  the  speeds  will  be  given  by 

■  -  Vt  sin  © 

V  “V.  cos  © 
y  t 

These  equations,  when  implemented,  generate  the  true 
trajectory. 

The  routine  also  computes  the  measurement  noise 
times.  The  Gaussian  noise  is  generated  by  computing  the 
linear  approximation  of  the  inverse  of  the  normal  curve  [93. 
The  input  comes  from  a  sequence  of  uniformly  distributed 
numbers  generated  by  the  equation  below 

r(I  +  1)  *  r  <  i )  *  317.  mod  ( 1 )  . 

The  resultant  sequence  is  zero  mean  and  is  scaled  to  obtain 
the  desired  variance. 

3.  The  Plotting  Routine  -  PLOT 

The  plotting  routine  is  contained  in  the  file  PLOT.C 
(IBM-PC),  and  was  written  as  an  independent  module  to  be 


linked  with  BIOSIO.OBJ,  and  reads  data  tram  th»  files 
XKK.DAT,  PKK.DAT,  XDIFF.DAT ,  TRJ.INP,  and  TRJ.DAT. 


That  is  a  mtnu  dr Ivan  program*  which  provides 
fourteen  (14)  different  options  and  was  designed  to  simplify 
the  analysis  of  data  generated  in  a  torpedo  run.  The  plats 
have  their  titles  already  chosen.  The  scales  reflect  the 
maximum  and  minimum  values.  A  reference  number  and  a  label 
far  SINGLE  or  MULTIPLE  array  cases  is  printed  on  the  plot 
for  reference.  Linear  interpolation  is  used  to  interconnect 
the  sample  data  points.  An  optional  plot,  where  the  data 
points  are  simply  plotted,  is  also  available.  This  last 
option  would  allow  the  time  to  be  associated  to  each  point 
on  the  curve. 

In  an  attempt  to  provide  the  CIE  machine  with 
similar  capabi 1 i ties,  an  ANSI  driver  was  also  defined  and 
tested  successfully  in  the  IBM.  However,  that  provides  a 
very  limited  graphics  capability,  allowing  only  SO  columns 
by  25  rows  of  resolution  against  the  640  by  200  in  the  IBM, 
in  graphics  mode.  The  file  KR1.C  makes  use  of  the  defined 
driver  to  produce  the  graphics  presentation  of  the  estimated 
trajectory. 

For  UNIX  systems,  like  the  CIE,  one  might  want  to 
consider  the  "termcap"  database  of  hundreds  of  encoded 
descriptions  for  terminal  control.  However,  except  in 
unusual  circumstances,  C's  standard-1 i brary  I/O  functions 


provide  excellent  portability  and  a  wida  range  of  display 
features  C61  . 


4.  BI08I0. ASM  and  BIOSIQ.QBJ 

Thasa  files  contain  tha  assambly  languaga  coda 
(public  domain)  and  tha  objact  coda  (IBM-PO  to  interface 
tha  C  programs  with  tha  kayboard,  display,  output  ports  and 
to  accass  an  absoluta  mamory  location. 

5.  DOST I HE. ASM  and  DOSTIME. OBJ 

Thasa  files  contain  tha  assambly  language  code  and 
tha  objact  coda,  respect i valy,  that  allow  accass  to  the 
internal  clock  (IBh-PC). 

6.  Tha  S1MTERM  program 

This  is  a  public  domain  program  written  by  Jim  & 
Eric  Holtman  that  allows  one  to  emulate  an  HP-like  terminal 
to  a  UNIX  system.  It  is  a  sel-f  contained  module  that  allows 
one  to  connect  tha  IBM-PC  to  tha  CIE  microcomputer ,  directly 
by  using  tha  RS-232  output  on  the  IBM  and  one  o-f  -four  (4) 
output  ports  on  tha  CIE. 

7.  The  KALMAN. H  header  file 

It  contains  the  de-finitions  of  the  program 
constants,  initial  values  of  parameters,  and  the  definition 
of  the  ANSI  terminal  driver,  which  can  be  used  in  the  future 
to  generate  rough  plots  on  most  of  the  nongraphic  terminals. 


APPENDIX  B 


ALGORITHM  FOR  THE  KR.C  PROGRAM 


#define  CONSTANTS 
#def  i  ne  MACROS 
#include  FILE. H 
double  xkkC2003; 
mai n  O 
C 

/*  DEFINING  FILES  */ 


/*  used  to  de-fine  constants  */ 
/*  de-fine  macros  */ 

/*  include  header  -file  */ 

/*  declare  global  variables  */ 
/*  main  prog,  declaration  */ 

/*  block/function  delimiter  */ 


FILE  *fpxyz,  *-fopen<)|  /*  declares  fpxyz  as  a  */ 

.  /*  pointer  to  a  file  */ 

.  /*  yet  to  be  open  using*/ 

/*  fopenO  function  */ 


/*  DECLARATION  OF  VARIABLES  */ 
int  i,j;  /*  long  integers  ■*/ 

short  ii,jj;  /*  short  integers  */ 

float  x,y;  /*  single  precision  •*/ 

double  xx, yy;  /#  double  prec.  reals  */ 

/*  OPENING  FILES  */ 


f  pp»f  open  (  "pkk.  dat 11 ,  "m"  )  ;  /*  open  file  pkk.dat  */ 

/*  for  writing  */ 

f pe»f open ("xdif f . dat" , "w" > ;  /*  open  xdiff.dat  */ 
fpxyz»fopen("xkk.dat","w">  ;  /*  open  xkk.dat  */ 

/*  INITIALIZATION  OF  VARIABLES  */ 

Initialize  HYDRO C  3 
Initialize  gammaCl 
Initialize  covwC3 
Initialize  xkkmlCl 
Initialize  xbE 3 ,ybC 3 ,zb C 3 
/*  DATA  INPUT  -  INTERACTIVE  */ 
menu:  Put  menu 

Get  inputs 

Performs  actions 
Case  statement 

1  Run  trajectory 

Write  run#  and  xkkmlC3  to  trj.inp 
Call  trajec  to  read  trj.inp  and 

generate  noisy  trajectory 

2  Run  Kalman  filter 

3  Modify  Parameters 

a.  Optimization  criteria 

b.  Single/multiple  array 

c.  Initial  x,  y,  z 

d.  Initial  Vx ,  Vy 

e.  Noise  covariance 


f .  Error  covananci 

g.  Redefine  maneuver 

-  Turn  rata  based 

-  G  number 

-  Start /end  time 

h.  Main  menu 
/*  Run  Kalman  -filter  #/ 

Sat  run  #  -for  re-ference 

Sava  inputted  data  into  -files 

□pen  -filas  trj.dat  and  zic.dat  -for  reading 

Initialize  QFIND  block 

Initialize  hrowCl 

Initialize  pdumCl 

Initialize  rrCl 

Prepare  screen  -for  real  time  presentation 
Calculate  scaling 
Draw  -frame 

Get  time  -from  internal  clock 
/*  START  TIME  LQOP  */ 

Start  time  loop  :  -for  (kk»0;  kk<i time;  kk++) 

Get  hydrophone  array  coordinates 
again:  Get  tima 

I-f  time  interval  <  1.31  goto  again 
Update  time 

Get  trua  timas  a  true  positions  (noisy  data) 
Read  dataCl  -from  -file  trj.dat 
Read  zicCl  -from  -file  zic.dat 
Save  true  positions  in  trued 
adapt:  /*  GET  HROW,  CALCULATE  GAIN,  ESTIMATE  */ 

/*  COVARIANCE  OF  ERROR  SEQUENC I ALLY  * 
js:  for  (i»0;  i<4;  i++) 

t 

Calculate  denom  using  double  prec.  sqrt 
Calculate  hrowCOl,  hrowC21,  hrowC41 
Calculate  gnumCl,  gdenom 
Calculate  gi Cl  =  gain  column 
Calculate  covariance  of  error  pi  Cl 
for  (i i =0; i i <25; i i++) 


calculate  pdumCl 

> 

calculate  pi C 1 

Calculate  measurement  prediction 
calculate  zhat=denom/4860. 
calculate  zdif f Ci l=zicCi 1-zhat 
Compute  3sigma  gate 

p=max (piC01,piC121,piC241) 

/*  use  single  precision  sqrt  */ 
gatea3. * (sqrt (p) /4860  +  rrCil) 
Edit  invalid  time  measurements 


Put  gain,  giC3»0 

Calculate  estimate  based  on  one  meas. 

prediction 

■for  (ii=Oj  i i<5j  i i++) 

xi£ii3axkkml£ii  3+gi  Ci i 3*zdi f f  Ci 3 
/*  Update  for  next  column  */ 
for  <ii=0; ii<5$ ii++) 

£ 

xkkml Ci i 3*xi Ci i 3 
pkkmlCii3apiCii3 
> 

for  <ii™5; ii<25; ii++) 
pkkmlCi i 3»pi Ci i 3 
if  <i«3>  goto  156 
>  /*  end  of  js  loop  */ 

/*  Accept  values  as  updates  for  time  */ 

/*  interval  */ 

Do  updates  for  the  time  interval 
xkkml C  3»xi C  3 
pkkml C  3*pi C  3 
xkkC  3a>xi  £  3 
pkkC3=pi C3 

Recalculate  time  measurements  and  form 
absolute  value  of  residuals 
Edit  invalid  times  for  adaptive  maneuver 

routine 

Compute  adaptive  □  matrix 

If  <icurve«l)  compute  adaptive  Q 
else  use  constant  Q 
Check  time 

By  pass  maneuver  if  no  time  left 
Check  for  valid  measurements 
By  pass  adapt,  maneuver  if  all  invalid 
By  pass  maneuver  if  not  steady  state 
Check  average  zdiffCi3  vs.  zdifav 
By  pass  maneuver  if  avg.<zdifav 
/*  Adaptive  maneuver  */ 
pkkmlC3=pkkC3+qC3 
/*  Reiterate  same  time  slot  */ 
goto  adapt 

Compute  measurement  errors 
xdiff C03»xkkC03-truedC03 
xdi  f  f  C 1  3**x  kk  £23 -trued  C  1  3 
xdiff C23»xkkC43-truedC43 
Compute  switch  to  change  between  Q 

constant  s  straight  line  trajectory 
or  Q  adaptive  s  circular  trajectory 
Calculate  predictions  for  the  next  time 
pkkmlC3  and  xkkmlC3 
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SETTING  UP  FOR  EXECUTION 

A.  USER'S  NEEDS 

This  section  describes  the  actual  hardware  and  software 
utilized  to  produce  the  results  in  Appendix  H. 

HARDWARE 

The  -Following  describes  the  hardware  utilized  and  the 
way  to  interconnect  the  IBM-PC  and  the  CIE  microcomputer  . 

IBM— Pc  :  2  -floppy  disk  drives,  10  megabytes  hard  disk, 

graphic  board,  graphic  color  display,  serial  port  (RS-232) , 
8087  math  coprocessor  chip  and  Epson  parallel  printer; 

CIE  680/30  :  20  megabytes  hard  disk,  4  serial  output 

parts,  Amdek  monitor  and  keyboard. 

The  computers  were  interconnected  by  cable,  -from  the  IBM 
RS-232  output  port  to  one  o-f  the  4  ports  on  the  CIE. 

SOFTWARE 

The  -files  and/or  programs  utilized  are: 

1.  SIMTERM  -  communications  program  (in  the  IBM).  Allows 
transferring  files  between  the  IBM  and  the  CIE. 

2.  CIE. BAT  -  batch  file  (IBM).  Executes  the  necessary 
sequence  to  interconnect  the  IBM  and  the  CIE.  Needs  SIMTERM. 

3.  KR.EXE  -  executable  file  that  contains  the  program  to 
simulate  the  torpedo  trajectory  and  the  Kalman  filter 
program  (IBM).  The  trajectory  routine  is  executed  directly 
from  the  KR.C  program  as  a  menu  option  and  it  generates  the 


data  files  "trj.dat",  and  "zic.dat".  Then  the  trajectory  and 
time  information  from  the  files,  “trj.dat"  and  "zic.dat" ,  is 
used  by  the  Kalman  filter  algorithm  to  produce  estimates.  It 
generates  the  files  "xkk.dat",  "pkk.dat",  "xdiff.dat", 
"trj.inp"  and  "par am. dat " .  At  the  end  of  the  run,  a  total  of 
7  files  would  be  generated. 

4.  PLOT.EXE  -  executable  file  that  contains  the  plotting 
routine  (IBM).  Allows  posterior  graphic  analysis  of  the 
results.  Uses  the  information  contained  in  the  files 
"xkk.dat",  "pkk.dat", "xdiff.dat", "trj.dat"  and  "trj.inp". 

5.  KAL.BAT  -  batch  file  (IBM).  Executes  the  necessary 
sequence  to  run  KR.EXE  and  PLOT.EXE. 

6.  KALMAN.H  -  header  file  (IBM  and  CIE) .  Contains 
definitions  used  by  CIEKR1.C  ,  KR.EXE,  PLOT.EXE,  TR. EXE  . 

7.  a. out  -  executable  file  generated  from  compilation  of 
CIEKR1.C  on  the  CIE  machine. 

B.  PROGRAMMER'S  NEEDS 

It  is  highly  recommended  to  have  a  good  full-screen  text 
editor,  and  the  source  code  files  included  in  the  directory 
of  the  IBM-PC.  Source  code  files  have  extension  " .C" .  A  good 
C  compiler  (IBM)  is  also  recommended. 

The  software  required  is: 

1.  LATTICE  C  or  MICROSOFT  C  compiler  ; 

2.  FULL  SCREEN  EDITOR  ; 

3.  SIMTERM  -  terminal  emulation  /  communication  program; 


4.  SOURCE  files  i  TR.C,  KR.C,  PLOT.C  ; 

5.  HEADER  -files  I  KALMAN. H,  MATH. H,  STDIO.H  ; 

A.  ASSEMBLY  LANGUAGE  ROUTINES:  BIOS 10. ASM,  DOSTIME. ASM  ; 

7.  IBM  MACRO  ASSEMBLER  ; 

8.  MICROSOFT  LINKER  . 

Using  the  Lattice  C  compiler  requires  the  files  LC1.EXE, 
LC2.EXE,  LCS.LIB,  LCMS.LIB,  CS.OBJ,  MATH. H,  STDIO.H  to  be 
present.  The  assembly  language  routines  must  first  be 
transformed  into  object  files  by  using  the  Macro  Assembler. 
The  file  TR.C  must  also  be  compiled  using  LC1  and  then  LC2 
to  generate  the  TR.OBJ  file.  These  are  all  linked  to  the 
file  KR. OBJ  to  produce  the  executable  file  KR.EXE  . 
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COMPILING,  EXECUTING,  TRANSFERRING  FILES,  AND  PLOTTING 
A.  RUNNING  THE  PROGRAM  ON  THE  IBM-PC 

This  will  assume  one  is  running  the  Kalman  Filter 
program  on  the  IBM  PC/AT  with  a  hard  disk  drive  defined  as 
drive  "c"..  The  system  prompt  is  ,,c>" ,  and  the  required 
-files  are  in  the  subdirectory  named  "KALMAN". 

I-f  you  are  not  in  drive  "c",  change  to  the  correct  drive 
typing  "c: "  and  hit  "RETURN".  The  system  will  then  answer 
with  the  prompt  "c>".  Type  "cd  kalman"  to  access  the 
directory  KALMAN  . 

1.  Compiling  KR.C 

This  will  assume  you  are  running  the  Lattice  C 
compiler.  Type  "ce  kr"  to  compile  the  file  KR.C,  using  the 
batch  file  "CE. BAT" .  The  files  KALMAN. H,  DOSTIME. OBJ, 
TR.OBJ,  BIQSSIQ. OBJ ,  LINK. EXE,  CE. BAT,  and  KR.C  need  to  be 
in  the  same  directory. 

The  Appendix  E  contains  a  listing  of  the  file 

CE.BAT. 

2.  Executing  KR. EXE  and  PLOT. EXE 

Type  "KAL",in  drive  "c",  to  execute  the  batch  file 
KAL.BAT.  The  required  sequence  of  commands  is  initiated  and 
can  be  interrupted  by  pressing  "CTRL-BREAK"  ("CTRL"  and 
"BREAK"  keys  at  the  same  time).  The  Appendix  E  contains  a 
listing  of  the  file  KAL.BAT  . 
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The  first  file  to  be  executed  is  GRAPHICS.  It  allows 
dumping  IBM-PC  graphic  screen  to  the  printer. 

Next  the  file  KR.EXE,  containing  the  trajectory 
routine  and  the  Kalman  Filter  program,  is  executed.  Menus 
give  the  indication  for  data  input,  to  run  the  trajectory 
routine,  and  to  run  the  Kalman  Filter.  One  normally  defines 
the  parameters  first  (option  #  3) ,  then  run  the  trajectory 
routine  (option  #1),  and  then  run  the  Kalman  Filter  (option 
#  2). 

After  the  run,  the  plotting  routine  ("Plot")  is 
called  to  produce  the  graphic  output.  Up  to  fourteen 
different  plots  can  be  generated  by  making  the  desired 
selection  from  the  menu.  The  output  to  the  printer  is 
obtained  by  pressing  "SHIFT-PRTSC"  ("Shift"  and  "PrtSc"  at 
the  same  time). 

If  a  plot  ends  abnormally,  for  instance  by  the  use 
of  CTRL-BREAK,  the  screen  is  left  in  the  graphics  640  by 
200  mode,  then  the  command  "mode  bwSO"  will  set  the  display 
mode  to  Black  and  White  with  80  characters  per  line. 

B.  RUNNING  THE  PROGRAM  ON  THE  CIE 

Turn  on  the  system  with  the  floppy  disk  drive  open.  This 
will  force  the  system  to  boot  and  accept  the  internal  hard 
disk  drive  as  the  current  drive.  Turn  on  the  terminal 
display  and  when  the  cursor  appears,  press  “ENTER".  Follow 
the  prompts  for  date  and  time  input. 


Aft er  booting,  the  acrwn  Mill  show  ths  message  "CIE 
GRAPHICS  SYSTEM  #  7"  at  tha  top,  and  “login  i  "  at  tha 
bottom.  At  this  momant  one  should  have  tha  SIMTERM  diskette 
in  drive  A  (tha  one  at  tha  left)  on  tha  IBM-PC.  If  tha 
logged  drive  is  not  A,  ona  should  type  "as"  and  press 
“ENTER”  to  change  to  drive  A  .  In  driva  A,  one  should  type 
“cie"  and  “RETURN"  (ENTER).  The  batch  file  CIE. BAT  will  send 
the  correct  command  line  to  connect  tha  IBM  and  the  CIE. 
Make  sure  the  cable  connection  is  already  done. 

The  message  "connection  established"  will  then  appear  on 
the  screen.  Press  “ENTER"  to  get  “logim".  Log  in  using  the 
account  "Jose",  and  press  "ENTER"  for  the  password.  The 
system  prompt  is  "7."  .  Change  directory  using  "cd  work"  . 
Ona  is  then  ready  to  transfer  files  or  to  execute  a  program 
in  the  CIE  machine. 

1 .  Transferring  Files  from  the  IBM  to  the  CIE 

To  transfer  a  file  use  tha  redirection  capability  of 
the  Unix  system  and  the  command  "cat". 

Type  "cat  >  f i 1 ename. ext "  and  press  "ENTER",  at  the 
system  prompt  ("%").  The  cursor  will  stop  at  the  begining 
of  the  next  line.  "ALT-D"  will  dump  a  file  to  the 
communication  line.  One  will  be  prompted  for  the  filename. 

If,  for  instance  one  wishes  to  transfer  the  file 
"kalman.h",  which  is  in  drive  ci  (IBM),  to  the  CIE,  given  it 
the  name  "kalman.h",  the  following  sequence  should  be 


performed,  starting  at  the  prompt 


:  Type  "cat  > 


the  rum*  "kalman.h" ,  the  following  sequence  should  bo 
porformod,  storting  at  tha  prompt  "X"  i  Typo  “cat  > 
kalman.h"}  prass  "ALT-D" ;  typo  "c: kalman.h"  ,  whon  asked  for 
the  IBM  filename,  and  press  “ENTER"  to  start  the 
transmission. 

At  the  end  of  the  transmission  the  computer  bell 
will  ring,  signaling  buffer  overflow.  One  should  wait  until 
no  more  new  lines  are  being  written.  Press  “CTRL-D"  to  close 
the  file  in  the  CIE.  The  prompt  “X"  will  reappear  on  the 


screen . 


The  new  file  < kalman.h)  will  have  to  be  edited  on 


the  CIE  to  remove  the  inserted  empty  lines: 

Type  “ed  kalman.h"  and  the  editor  will  show  the 
number  of  lines  in  the  file.  Then,  type  ,,g//'*/d"  and  press 
“ENTER".  The  number  of  lines  in  the  file  will  be  reduced  to 
the  half.  This  will  not  affect  the  blank  lines  originally 
in  the  program,  just  the  ones  inserted  by  the  process  of 
transmission. 

Type  "w"  to  write  the  modified  program  version  to  the 
disk,  and  "q"  (quit)  to  leave  the  editor. 

2.  Compiling  on  the  CIE 

Type  the  command  line  "ce  -f  -v  -L  -7  ciekrl.c"  to 
generate  the  executable  file  "a. out", 
where  :  "cc"  invokes  the  C  compiler; 

"-f"  option  ,  informs  the  compiler  to  use  "fast 
floating  point  format"; 


"-v"  option  ... 

"-L"  option  instructs  ths  compiler  to  generate 
pointers  to  allow  long  jumps,  for  a  code+data  greater 
than  32  kbytes} 

"-7"  option  instructs  the  compiler  to  use  the  Unix 

version  7  standard  library. 

3.  Executing  CIEKR1 

Type  “a. out"  to  start  execution  and  then  follow  the 
menus  for  inputs.  Since  the  CIEKR1.C  program  does  not 
generate  circular  trajectories,  the  files  containing  the 
trajectory  data  (TRJ.OAT)  and  transit  time  data  (ZIC.DAT) 
will  have  to  be  transferred  from  the  IBM  to  the  CIE,  in 
order  to  run  the  Kalman  Filter  for  the  circular  trajectory. 
Be  sure  to  input  the  same  data  that  were  used  to  generate 
the  circular  trajectory  on  the  IBM,  and  then  select  option 
#1  (Run  Kalman).  For  straight  line  trajectories,  just  follow 
the  menus. 

4.  Plotting  the  Results 

Since  there  is  no  graphics  capability  built  in  the 
CIE,  the  only  way  to  obtain  plots  is  to  transfer  the  files 
XKK.DAT,  PKK.DAT  and  XDIFF.DAT,  obtained  on  the  CIE,  back  to 
the  IBM  and  then  use  the  routine  PLOT  to  generate  the 
desired  graphic  output.  The  procedure  is  described  below: 

"ALT-C",  during  the  SIMTERM  session,  will  open  a 


capture  file.  One  will  be  asked  the  name  of  the  capture,  or 
log  file.  The  drive  name  should  be  specified. 


Far  instance,  assume  one  wants  to  transfer  the  file 
XKK.DAT  from  the  CIE  to  the  IBM  in  order  to  use  it  later  to 
generate  plots.  Since,  the  routine  PLOT  reads  specific 
files,  be  sure  to  erase  the  old  file  XKK.DAT,  in  the  IBM, 
before  copying  the  file  XKK.DAT  from  the  CIE,  because  the 
same  name  will  be  used. 

Type  "list  -n  xkk.dat",  at  the  prompt  without 
pressing  "ENTER".  Pressing  "ALT-C" ,  will  start  the  capture 
mode.  Then  one  will  be  asked  the  name  of  the  log  file.  Type 
"cixkk.dat”,  and  press  "ENTER",  to  open  the  file  on  drive  C. 
The  command  line  "list  -n  ...  "  will  reappear.  Press 
"ENTER"  to  start  the  transfer  and  the  capture.  Press  "ALT- 
C"at  the  end  of  the  file  listing,  to  close  the  log  file. 

After  doing  similar  procedure  for  the  files 
XDIFF.DAT  and  PKK.DAT,  leave  the  account  on  the  CIE  by 
typing  "logout". 

Leave  the  SIMTERM  program  and  drop  the  line, 
pressing  "ALT-F2".  One  will  be  back  to  drive  A  on  the  IBM. 

Type  "cs",  to  change  logged  drive  to  C  . 

One  may  use  his  preferred  text  editor  to  eliminate 
the  undesired  lines  in  the  files  XKK.DAT,  XDIFF.DAT,  and 
PKK.DAT.  The  files  should  contain  only  four  columns  of 
numbers,  except  for  the  first  row,  which  is  blank.  Then  one 
can  use  the  routine  PLOT  to  obtain  the  desired  plots,  by 
typing  "PLOT  “8000",  and  following  the  menu  options. 


/*#####*#######*#**#**###**#**######*******#****#*#»*##**##/ 
/*  */ 

/*  HEADER  FILE  -  KALMAN. H  */ 

/*  */ 

/*****#***##**#***##**##*#*****#*#*******#«#*******-»****#*** 


#def i ne 
#def ine 
#da-f  i  na 


nmax  5 
maxsq  25 
js  4 


#define  hcmax  12 


#def ine 
#def i na 
#def  i  na 
#def ine 
#d  e-fine 

#def ine 
#def  i  ne 

#def i ne 
#def ine 
#def i na 
#def  i  na 
#def ine 
#def  ine 
#def ine 
#def  ine 
#de-f  i  ne 
#def i ne 
#def i ne 
#def ine 
#def i ne 
#def  i  ne 
#de-f  ine 
#def ine 
#def ine 
#def i ne 
#da-f  i  ne 

#def ine 
#de-f  ine 
#def ine 
#def ine 
#da-f  ine 
#d  e-fine 
#def i ne 
#de-f  ine 
#de-f  ine 


hmax  72  /* 

dmax  3 

itime  150  /* 

npt  150 

i  cniax  5  /* 

/* 

cri ter  0. 1  /■* 

na  0  /* 

/* 

xpos  10000.0  /* 

xvel  -50.0  /* 

ypos  0.0  /* 

yvel  5.0  /* 

zpos  0.0  /* 

swO  33000.0  /* 

awl  27000.0  /* 

sw2  21000.0  /* 

sw3  15000.0  /* 

sw4  9000.0  /* 

sigdiv  1.0 
see  0.387463094  /* 
sacc  36.2 

vel  4860.0  /* 

twopi  6.28315308 
rrvar  1 . e-5 
rvar  l.e-10 
pkkdiag  1000.0 
1  i  invar  1 . 5 

/* 

gmO  0.86 
gml  1.31 

cwO  1.0  /* 

cwl  1.0  /* 

cw2  1.0 
xminl  -5000.0 
xminh  0.0 
xmaxl  5000.0 
xmaxh  40000.0 


#  o-f  col.  in  pkk,  q  ...  */ 

siza  of  equiv.  vector  PKK,Q. . . •*/ 

#  hydroph.  in  any  hydrophone  */ 
array  */ 

#  of  col.  in  matrix  HYDRO  of  */ 
hydroph.  t  -nord.  */ 

equiv.  vector  size  of  HYDRO  */ 

max.#  of  time  slots  */ 

max.#  of  optimal  iterations  *7 
allowed  */ 

criteria  to  compare  ZDIFAV  */ 
may  assume  ONLY  values  0  or  6  */ 

0  =>single|6*>  multiple  arrays  *7 
initial  torpedo  x-coordinate  *7 
initial  torpedo  x-velocity  */ 
initial  torpedo  y-position  */ 
initial  torpedo  y-velocity  */ 
initial  torpedo  z-position  */ 
swi  define  points  where  */ 
switching  from  one  to  */ 

another  */ 

array  */ 

is  done.  *7 

-  in  radians...  */ 

speed  of  sound  *7 


/#  defines  threshold  to  use  Q*/ 
constant  */ 


initial  was  1036.84  *7 
initial  was  1036.84  *7 


tdefina  yminl  -5000.0 
#d*fin*  yminh  -5000.0 
#d«fin«  ymx  1  5000.0 
#d*fins  ymaxh  5000.0 


/*****»******«********»********«*«********«**«*«#*******««*/ 
/*  */ 

/*  DEFINE  ANSI  DRIVER  */ 

/*  #/ 

/»*#*******#**#***********«*•»*•»•»•»****•»**#*■»*•»*■»*■»-»**-»*-»«*#** 


/*  #d«f ine  STD  IN  0  */ 

/*  #d«fin«  STDOUT  1  */ 

/*  #def  i  ne  STDERR  2  */ 

#d«fin»  CLR_SCRN  f  put* < "\33C2J" ,  stdarr) 

ftdafine  CUR_MV (row, col )  f print*  (atdarr ,  "\33C%dj  JidH" ,  row, 
col ) 

#d«f in«  CUR_UP<num>  fprintf  (atdarr ,  "\33C34dA"  ,  num) 

#dafina  CUR__DN<num)  fprintf (atdarr , "\33CXdB" ,  num) 

#dafina  CUR_RT  (num)  fprintf  (atdarr ,  "\33C5CdC"  ,  num) 

#dafina  CUR_LT  (num)  fprintf  (atdarr ,  "\33C%dD"  ,  num) 

#dafina  CUR_SKIP  f puta ( "\n" ,  atdarr) 

#daf  i na  CUR_SAVE  fputa ( "\33Ca" ,  atdarr) 

#daf ine  CUR_REST  f puta ( "NSSEu"  ,  atdarr) 

#daf i ne  CLR_LINE  f puta ( "\33CK" ,  atdarr) 

#dafina  BELL  fputc('\7',  atdarr) 

*dafina  CLR_E0S(r,  c)  {byta  i  j  CURJ1V(r,c);  \ 
for  (i_-rj  i_<«25|++i_>  CLR__LINE, 

CUR_MV (r ,c) j  > 


CURJDN  ( 1 )  ; 


/***********#****#**#*«*#***•»*****#*•»#***#*******#*•»*##«#**/ 
/*  */ 

/*  REAL  TIME  KALMAN  FILTER  PROGRAM  -  KR.C  */ 

/*  */ 

/***#**«****«****************#*******#**************«**-»**** 

#includ«  "stdio.h" 

#includa  "math.h" 

#include  "kalman.h" 
doubla 

x  kkml Cnmax 3 , pkkml Cmaxsq3 ,  pkkCmaxsq 3 ,xkk Cnmax 3 , dataCdmax 3 ; 


/*  STARTING  THE  MAIN  PROGRAM...  */ 


main  ( ) 


/*■»•»■»■»■»***■»*■»**#■»■»*•»■***•»■»■»*■»•»■»******#*■»•»■*■»■»■*■*•*•»•»■»***■»**•»■(•••»*/ 
/*  */ 

/*  VARIABLE  DECLARATIONS  */ 

/*  */ 

/*■***#■****■»***■**■»*•»•****■***•*•»#•»**■»***■»■»■»•»■»■*■»*■»•»***•»•»**•»■»■»■»■»■»■» 

FILE  *fpxyz,  *fopen<); 

FILE  *fpe,  afopenO; 

FILE  *fpp ,  *fop«n(); 

FILE  afptrj,  afopanO; 

FILE  afpzic,  *-fopen(); 

FILE  afparam,  afopenO; 

FILE  *fpin,  afopenO; 

int  i hours, i mi ns, i secs, it ha, col ,row,maxopt; 

int  i 8, i , j , kk , i in , icon t , i curve, opt ion; 

int  param, tr jopt ,nrange; 

int  i  i  ,  j  j  ,narray ,  nzdi  f  f  ,  in ,  p  li  ,  runn  ,c; 

int  jstart, jend,worg; 

float  rabs,zgate; 

double  height,width,ymax ,ymin,xmax ,xmin; 
double  xscale,yscale,bx ,by; 

doubl a  hours , mi ns , secs , ths , t i me , ol dt i me , del  tat ; 

double  sqrt ( > , oldx , value, xO,yO,zO, vxO, vyO; 

doubl e  denom , a 1 4 , gdenom , gat e , z di f av ; 

double  den ,velm, temp; 

double  a2,gl ,g2,g3,pl ,xdif f C33 ; 

double  zhat,zcs,dk,sigcc,sigacc; 

double  v, trued C33fwd,g; 

double  x ,y ,z ,auxh,xd,yd,zd; 

double  al ,el ,el2,e2,bb ,dd,bd; 

double  pkl , pk3 , pk5,pgate,rgate ,p ,p 1 1 ; 

double  phipkkCmaxsql ,pktempCmaxsq3 ,qCmaxsq3; 


double  pi Cmaxaq3 , pdumCmaxaq3 , qtempCmaxaql ; 
doubls  gnumCnmax 3 , gi Cnmax  3 , xi Cnmax 3 , hrowCnmax 3 ; 
double  zicCjs3,ziCj33,zdi-f-fCjs3,rCja3,phiaCjs3,2tCj335 
double  xbCja3,ybCja3,zbCja3,rrCja3; 
double  hydroChmax 3 ,covwC33 ,gammaC23 , optC53 ; 

/**#***************#******#********■*  ■*•**#•»■»***•****■#■*•**•*  ■*•***•»/ 
/*  */ 

/*  INITIALIZATIONS  */ 

/*  */ 

/********#*****#*#***####**#**#******#***#**#*******-*******/ 

•fpxyz=fopen  ("xkk.dat"  ,  "w"> ; 

-fpe=-f  open  <  "xdif  f .  dat "  ,  "w" ) ; 
f  ppa-fopen  ("pkk.dat"  ,  "w"> ; 

/*  INITIALIZE  CONSTANTS,  HYDROPHONE  MATRIX,  PHI,  R, 
DATA  FOR  TRAJECTORY,  AND  LOAD  X(0/-1>,  P(0/-1) 

*/ 

nzdi-f-f=4;  icont=0;  icurve=l;  value=criter;  wd=5.730; 
runn=l;  narray=na;  param=0;  worg=l;  g=0. 156;  nrange=0; 
jstart=40;  jend=80;  iS=0;  maxopt=icmax ; 


hydroC03=36000. 0; 
hydroC33=36030.  0; 
hydroC63=36000.0; 
hydroC93»36000.0; 


hydroC 13=  6000.0; 
hydroC43=  6000.0; 
hydroC73=  6030.0; 
hydroC 103=6000.0; 


hydroC 123=30000. 0;  hydroC 133=6000. 0; 
hydroC 153=30030. 0;  hydroC 16 3=6000. 0; 
hydroC 1S3-30000. 0;  hydroC 193=6030. 0; 
hydroC21 3=30000. 0;  hydroC223=6000. 0; 


hydroC23=  0.0; 
hydroC53=  0.0; 
hydroC83=  0.0; 
hydroC 11 3=  30.0; 

hydroC143=  0.0; 
hydroC 173=  0.0; 
hydroC203=  0.0; 
hydroC233=  30.0; 


hydroC243«24000. 0;  hydroC253=6000. 0;  hydroC263=  0.0; 
hydroC273=24030. 0;  hydroC2B3=6000. 0;  hydroC293=  0.0; 
hydroC 30 3 =24000. 0;  hydroC31 3=6030. 0;  hydroC323=  0.0; 
hydroC333=24000. 0;  hydroC343=6000. 0;  hydroC353=  30.0; 

hydroC363= 18000. 0;  hydroC373=6000. 0;  hydroC3B3=  0.0; 
hydroC393=18030. 0;  hydroC403=6000. 0;  hydroC413=  0.0; 
hydroC 423= 18000. 0;  hydroC433=6030. 0;  hydroC443=  0.0; 
hydroC453= 18000. 0;  hydroC463=6000. 0;  hydroC473=  30.0; 

hydroC483=12000. 0;  hydroC493=6000. 0;  hydroC503=  0.0; 
hydroC51 3=12030. 0;  hydroC523=6000. 0;  hydroC533=  0.0; 
hydroC543=12000. 0;  hydroC553=6030. 0;  hydroC563=  0.0; 
hydroC573=12000. 0;  hydroC583=6000. 0;  hydroC593=  30.0; 

hydroC603=6000. 0;  hydroC61 3=6000. 0;  hydroC623=  0.0; 
hydroC633=6030. 0;  hydroC643=6000. 0;  hydroC653=  0.0; 
hydroC663=6000. 0;  hydroC673=6030. 0;  hydroC683=  0.0; 
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hydro C 69 3 *6000. 0;  hydroC703*6000. 0;  hydroC713*  30. 

rC03*rvar;  rC13*rvar;  rC23*rvar;  rC33*rvar; 

gamma C03*gm0;  gamma C 1 3*gml ; 
covwC03»cw0;  covwC13=cwl;  covwC23*cw2; 

x0=xpos;  vx0*xv«l ;  y0*ypos;  vy0»yvel ; 
z0=zpos; 

■for  (i=0;  i<nmax ;  i++) 

■for  <  j=0;  J<nmax;  j++)  pkkml Ci*nmax+j 3=0. 0; 

■for  (i*0; i<nmax ; i++)  pkkmlCi*nmax+i 3-pkkdiag; 

phisC03*1.31;  phisC13=0.0;  phisC23*l . 31 ;  phisC33=0.0 

xbC03* (-15. 0) ;  ybC03»(-15. 0) ;  zbC03- (-15. 0> ; 

xbC  1  3*  15.0;  ybC 1 3* (-15. 0) ;  zbC 1 3* (-15. 0> ; 
xbC23= (-15. 0) ;  ybC23-  15.0;  zbC23* (-15. 0) ; 

xbC33— (-15. 0) ;  ybC33= (-15. 0) ;  zbC33=15.0; 


/*  INITIALIZATION  FOR  QFIND  #/ 

velm=l .  0/vel ; 

al4=phisC03; 

a2*al4#al4; 

gl*a2*0.5; 

g2=gl*gl; 

g3»al4*gl; 

si gacc=sacc*sacc ; 

si gcc=scc*scc ; 

■far  (i=0; i<nmax ; i++) 

far  ( j=0; j<nmax ; j++)  qCi*nmax+j 3=0. 0; 
qCmaxsq-1 3=sigdi v*a2*sigdi v; 

/*  INITIALIZATION  FOR  HROW  */ 
hrowC 13=0.0;  hrowC33=0.0; 


/*  INITIALIZATION  FOR  PDUM  */ 
for  (i i=0; i iCnmax ; i i++) 
i 

j j=i i*nmax ; 
pdumC j j+1 3*0. 0; 
pdumC j j+33=0. 0; 

> 

pdumC63=1.0;  pdumC 183=1 . 0; 

/*  INITIALIZING  RR  #/ 

for  (ii=0; ii<4; ii++)  rr Ci i 3=sqrt (r Ci i 3 ) ; 


/** 

/* 

/* 

/* 


**/ 

*/ 

*/ 

*/ 


DATA  INPUT 


/******«******«*****#*****#*«****###******#•»*##•»***#**•»**#*/ 
restart j 

xkkml C03-x0;  xkkml C 1 3»vxO;  xkkmlC23«yO;  xkkml C33«vyO» 
xkkmlC43«zO; 


menus  /*  OPTIONS  MENU  */ 
printf ("Xn") ; 

printf  ( "\n  KALMAN  FILTER  ">; 

print-f  ( "\n" )  | 

print-f  ("Xn  0PTI0N8  i“); 

printf ("Xn") ; 

print-f  <  "\n  1)  RUN  TRAJECTORY" > ; 

printf  <"\n") i 

printf ("Xn  2)  RUN  KALMAN") ; 

printf ("Xn") ; 

printf ("Xn  3)  MODIFY  PARAMETERS"); 

printf ("Xn") ; 

printf ("Xn  4)  QUIT"); 

printf  ("Xn") ; 
if  (param!»0) 

<. 

printf ("Xn") ; 

printfC'XnA  parameter  was  changed  ...  You  may  "); 
printf ("have  to  run  TRAJECTORY  before  continue."); 

> 

printf ( "XnEnter  value  (1-4)  s  "); 
scanf  ( "*/.d  ”  ,  Stoption)  ; 
if  (option»»l) 

f pin«f open ( "tr j . inp" , "w") ; 
fprintf  (f  pin,  "V.d  %d"  , narray  ,runn) ; 

f  printf  (fpin,  "Xn'/.e  %e  7.e", 

xkkmlC03,xkkmlCl3,xkkmlC2]) ; 
fprintf  (f  pin  y  "Xntte  /Ce",xkkmlC3D,xkkmlC41)  ; 
f close (fpin) ; 

trajec (narray , hydro, xb ,yb , zb , xkkml , j start , jend , wd , g , worg) ; 
param=0; 
goto  menu; 

J 

else  if  (option«2)  goto  run; 

else  if  (option««3)  goto  modify; 

else  if  (option=*4>  goto  quit; 

else  goto  menu; 


modify: 

printf  < "\n  MODIFY  PARAMETERS  OPTIONS  :">; 

printf ("Nn") ; 

printf <"\n  1)  ZDIFAV  stop  optimization"); 

printf <"  critsria"); 

printf (”\n  2)  SINSLE  /  MULTIPLE  ARRAY"); 

printf <"\n  3)  INITIAL  X,  Y,  and  Z  positions"); 

printf <"\n  4)  INITIAL  Vx  and  Vy  velocities"); 

printf <"\n  S)  NOISE  COVARIANCE  -  diag_elem  of") 

printf <"  R"); 

printf  ("\n  6)  MAX.  #  ITERATIONS  OF  OPTIMAL); 

printf <"  MANEUVER"); 

printf <"\n  7)  REDEFINE  MANEUVER  -  w  /  g  /  time) 

printf <"  interval"); 

printf <"\n  8)  USE  LOW  /  HISH  RANGE  ">; 

printf ("\n") ; 

printf ("\n  9)  MAIN  MENU  -  OPTIONS"); 

printf ("\n") ; 

printf ( "NnEnter  value  (1-9)  i  "); 
scanf  < "Xd" t&param) ; 

swi  tch  (par am) 

case  1 : 

printf  (  "\n  7.  s  */.e","  Current  CRITERIA  is:" 

value); 

printf ("Nnchange  ?  CY/N3  :"); 

scanf  ("Xs"  ,8«c)  ; 

if  (c»»'n'  !!  c**'N'>  break; 

printf ( "NnEnter  new  value  (real)  :  "); 

scanf  ("7.1f  "  ,  &  value) ; 

break; 

case  2: 

printf ( "\nSINGLE  array  is  the  default"); 
if  (narray«0) 

printf ( "NnXs" , "Current  case  is  :  SINGLE"); 
else  printf  ( "\n7.s" ,  "Current  case  is:  MULTIPLE") 
printf ( "Nnchange  ?  CY/ND  :">; 
scanf ( "%s" , &c) ; 
if  <c=»'n'  ! !  c== 'N ' >  break; 
printf ( "NnEnter  S  for  SINGLE  or"); 
printf  ("  M  for  MULTIPLE  array  :  "); 
scanf  <"y.s",S«c)  ; 

if  (c=*®'s'  !!  c==  'S')  narray=0; 
else  if  (c== 'm  '  i!  c== ' M ' )  narrayal; 
else  narray=0; 
break; 


cat«  3i 

printf  ("NnCurrent  values  are  :  ">; 
pr  i  ntf  ( " NnXs  Xa  Xs  Xa  Xs  Xa"  , "xpos  -  ", xkkml C03 
11  ypos  ■  "  ,xkkml  E23  ,  "  zpos  •  ",xkkmlE43> 
print-f  ("\nchanga  ?  CY/N3  *">; 
scanf  ("Xs",&c>  ; 
if  (c—  'n'  II  c—'N'>  break; 
print-f  ("\nEntar  naw  valuas  i  ">; 
print-f  ( "\nXs"  ,  "xpos  ■  " 
scan-f  ( "Xlf  "  ,8<x0)  ; 

printf ("NnXs", "ypos  ■  " ); 
scan-f  <  "Xl-f  "  ,&y0) ; 

printf ("NnXs" , "2pos  ■  " ); 
scanf < "Xlf ”,&z0)  ; 

xkkmlCO 3»x0;  xkkmlC23=yO;  xkkml C43»zO; 
break; 
casa  4t 

print-f  ("NnCurrent  values  are  >  " >; 
print-f  ("NnXs  Xa  Xs  Xe" ,"xvel  *  ",  xkkml  Cl  3, 

"  yvel  »  ", xkkml C33> ; 
print-f  < "Nnchanga  ?  CY/N3  s"); 
scan-f  ("Xs"  ,$*c) ) 
if  (c*«'n'  II  c»-'N'>  braak; 
print-f  ( "NnEnter  new  values  s  "); 
print-f  ("NnXs","xval  -  ">; 
scanf ( "XI f " , &vxO) ; 

printf ( "NnXs" , "yvel  ■  " ); 
scanf ("Xlf " ,&vyO) ; 

xkkml  C  1  3=*vx0;  xkkml  C33»vy0; 

break; 

case  3: 

printf ( "NnCurrent  values  are  s  "); 
printf ("NnXs  Xe  Xs  Xe  Xs  Xe  Xs  Xe","rC03  *  ", 
r C 0 3 , "  r C 1 3  =  ",rC13,"  rC23  =  ",rC23, 

"  r C33  -  ",  r C33 > ; 
printf ( "Xnchange  ?  CY/N3  s"); 
scanf ( "Xs" ,&c) ; 
if  (c»*»'n'  II  c®«'N')  break; 
printf ("NnEnter  new  values  s  "); 
printf ("NnXs" ,"rC03  =  "); 
scanf  < "XI f " .&r C03 ) : 


print#  ("NnXs"  ,  "rC13  -  "); 
scan#  ("Xl#",lcrC13) ; 

print#  ("\nXs"f"rC23  «  "); 
scan#  ("Xlf "  f &r C23)  f 

printf  ("NnXs,V,rC33  -  '*); 
scan#  ("Xlf"  ,&rC33) ; 

#or  (ii-0| ii<4; ii++)  rr Ci i 3«sqrt (r Ci i 3 ) ; 
brsak; 

case  6t 

print# < "\nCurrant  valu*  is  i  " ); 

printf ( "NnXs  X  d","#  o#  iterations  =*  " ,maxopt); 

print# ("Nnchange  ?  CY/N3 

scan#  <"%s",8«c)  ; 

i#  (c~'n#  Si  c-«'N'>  break; 

print# ("NnEnter  new  value  :  "); 

print#  ( "NnXs"  ,  "max opt  #  ■  ’* ); 

scan# ( "Xd" , &maxopt ) ; 

break; 

case  7s 

trmenus  print# ("\n  DEFINE  MANEUVER  \n">; 

print# <"\n  1)  Define  maneuver  based") ; 

print# ("on  turn  rate"); 

print# <"\n  2)  Define  maneuver  based"); 

print# ( "  on  g-number " ) ; 

print# <"\n  3)  Redefine  start  and  end"); 

print# <"  time  slots"); 

print# <"\n  4)  Modify  menu"); 

print# ( "\n\nOpt ion  s  "); 

scan# ( "Xd" ,&tr jopt) ; 

i#  (trjopt»4)  break; 

i#  (trjopt8**!) 

<. 

printf ("NnCurrent  value  is  :  "); 
printf ("Xs  Xe"f"turn  rate  s  w  *  " ,  wd); 
worg»l ; 

print# <"\n  Enter  new  turn  rate  <dg/s)s") 

scan#  ("*/.l#"  ,&wd) ; 

> 

else  i#  <trjopt«2) 

< 

print# < "NnCurrent  value  is  s  "); 
print#  ("Xs  Xe" ,  "g-number  :  g  ■  "  ,  g>; 
worg«0; 

print# ("Nn  Enter  new  g-number  :  "); 

scan#  ("7.1#"  ,&g) ; 


'**  I4-. Jtg'  Jl  .♦li 
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alsa  if  <trjopt»«3> 

< 

printf  ("XnCurrent  valun  are  1  "  >; 
printf <"\nXs  Xd  Xs  Xd" , "tstart  -  ", 

printf ("jstart, "tend  ■  " ,jend); 
printf <"\n  Enter  start  time  slot  1  “>; 
scanf ( "Xd " , & j  st ar t ) ; 

printf ("\n  Enter  and  tima  slot  :  " ) ; 
acanf  < "Xd" ,& jand) ; 

> 

goto  trmenu; 
casa  Si 

printf < "\nLQW  range  is  tha  default"); 
printf <"  (use  single  array)"); 
if  <nrange««0) 

printf <"\nXs" , “Currant  casa  is  t  LOW"); 
alsa  printf <"\nXs", "Current  casa  is  1  HIGH"); 
printf <"\nchanga  ?  CY/N3  1"); 
scanf ( "Xs" ,&c) ; 
if  (c“‘n  *  !i  c»-'N')  break; 
printf (“NnEntar  L  for  LOW  or"); 
printf <"  H  for  HIGH  range  1  "); 
scanf  ("Xa ",8<c) ; 

if  <c—'l'  SI  c— 'L')  nranga-O; 
alsa  if  <c»*'h'  !!  c~'H*)  nrange«l; 
alsa  nrange-O; 
break; 

casa  9 1 

goto  menu; 
break; 

default  1 
break; 

> 

goto  modify; 

/*  START  EXECUTION  */ 
printf  <"\n") ; 

printf ( "\nEnter  run  #5  "); 
scanf  < " Xd  "  ,  8<r unn ) ; 

f param«f open < "param. dat" , "w" ) ; 

f printf (f pxyz , "X2d" ,runn) ; 
f printf  <f pa, "X2d" ,runn) ; 
fprintf (fpp, "X2d" ,r unn) ; 
f printf (f param, "X2d" ,runn) ; 
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if  (narray*«0) 

{ 

fprintf  (fpxyz,"Xs“,"  Single") ; 
fprintf  <f  pe,  "Xs"  ,  "  Single"); 

■fprintf  (fpp^'Xs","  Single") ; 
fprintf (f par am, "Xs" , "  Single") ; 

> 

else 

fprintf (fpxyz,"Xs","  Mult"); 
fprintf (fpe,"Xs","  Mult"); 
fprintf  <fpp,"7.s"f"  Mult"); 
fprintf  <fperamf  "7s 11 ,  "  Mult") ; 

y  */ 

fprintf (fparam, "\n  Xs  Xe" , "CRITERIA  ■  value); 
fprintf (fparam, "\nXs  Xe  Xs  Xe  Xs  Xe" , "xkkml COD  =  ", 

xkkmlCOD,"  xkkml C ID  »  " ,xkkml C 1 1 , 

"  xkkmlC23  »  " ,xkkmiC2D) ; 

fprintf (fparam, "\nXs  Xe  Xs  Xe" , "xkkml C33  -  ",xkkmlC3D, 
"  xkkml C43  ■  ", xkkml C4D > ; 
fprintf  (fparam, "\nXs  Xe  Xs  Xe  Xs  Xe  Xs  Xe" , 

"r  COD  ■  ",rCOD,"  rCID  =  ",rClD,"  rC2D  =  ", 

r  C2D , "  rC3D  »  ",rC3D); 

fprintf (fparam,"\nXs  Xe  Xs  Xe  Xs  Xe" , "pkkml COD  =  ", 

pkkml COD , "  pkkmlClD  =  ",pkkmlClD, 

"  pkkml C2D  -  ",pkkmlC2D); 
f close (fparam) ; 

f  pin«*f  open ( "tr j . inp" , "w" ) ; 

fprintf (f pin, "X d  Xd" ,narray,runn) ; 
fprintf (f pin , "\nXe  Xe  Xe" ,xkkmlCOD ,xkkmlC2D , 
xkkml C4D ); /*posi ti ons*/ 

f  printf  (f pin ,  ”\nXe  Xe"  , xkkml  C 1 D  , xkkml C3D )  ;  /*vel*/ 

f close (fpin) ; 
if  (option»4)  goto  quit; 

fptr j»f open ( "tr j. dat " , "r"> ; 
fpzic»fopen("zic.dat","r") ; 

wait:  printf ( "\nPress  g  and  ENTER  to  start  ...  "); 
scanf  ("Xs"  ,8«c) ; 

if  (c!  =  'g*  &8<  c!='G')  goto  wait; 

/* 

if  (nrange»0) 

C 

xminaxminl;  x max =x maxi; 
ymi n»ymi n 1 ;  ymax  ™ymax 1 ; 

> 


Ef 


v. 


xmax>xmaxhy  xmin-xminhy 
ymax>ymaxhy  ymin>yminhy 
) 

h®i gh t >y max -ymi n  y 
wi d t h“x max -x mi n y 
x  seal ®>600 . 0/m1 dth y 
yscal®>-160. O/height y 
by«196.0-ymin*yscale+0. Sy 
bx  >600 . 0-x  max  *x  seal  ®+0 . 5  y 

biosini <6) y 
f ram® (36,0, 196,600) ; 
biossat (23, 1 ) y 

print*  ("X.  l*%s%.  1-f  "  ,xmin, "  , "  ,ymin)  y 
biosset  (6,61 )  y 

printf  ("X.  l-f  "/.*%.  1*"  , xmax  ,  "  ,  "  ,ymax )  y 
bioss®t (22,65) y 
print* ("%s  TCd","#  “  ,runn)y 
biossat (23,65) y 

i*  (narray— 0)  print*  ( “SINGLE" )  y 

•Is®  print* ("MULTIPLE") y  */ 

print* < "Nnwait  ")y 

/a*******************************##**##**#*****#***#******#/ 
/*  */ 

/*  KALMAN  FILTER  */ 

/*  */ 

/a#*********************************************#**********/ 

/*  START  THE  TIME  SLOT  LOOP  AND  SET  ARRAY  HANDOFF  */ 

/*  POINT  */ 

for  (kk»0y kk<itimey kk++) 

C 

dk=»kk+l  y 


dost i me  (&i hours ,  8<imins  ,&i  secs  , &i  ths)  y 

hour s=*i hours;  mins»imins;  secs»isecsy  ths=iths; 

time=3600. *hours+60. *mins+secs+0. 01#thsy 

del tat=time-oldtime; 

> 

while  (deltat  <  1.25  &&  kk>0) y 
oldtime>timey 

i*  (del tat >1 . 31 )  print* ("*"> ; 
els®  print* (". ") y 


VVVvy//. 


print*  ("\nXs  Xa'V't  -  %oldtima>  **/ 

print*  ("X*  Xd","  kk  -  "  ,kk>$  */ 

print*  < " \n*********************************** " ) ?  */ 

print* ("X20s  Xd  \n'V  *************  TIME  s",kk>s  */ 

/*  SET  HYDROPHONE  ARRAY  COORDINATES  */ 
i*  (narray  !*  O) 

£ 

i*  (xkkml £03  >  swO)  18*0} 

•Isa  i*  (xkkml COD  >  swl)  i8»12; 
else  i*  (xkkml £01  >  sw2>  i8-24j 
elsa  i*  (xkkmi£03  >  sw3)  i8*36; 

•Isa  i*  (xkkml £03  >  sw4>  i8*48j 
•Isa  i 8*60 | 

*or  (i*0| i<4| i++) 

£ 

j»3*i+i8; 
xbCi 3*hydro£ j3; 
ybCi 3*hydro£ j+13 ; 
zbEi 3*hydro£ j+23; 

> 

> 

/*  GET  THE  TRUE  TIMES  AND  THE  TRUE  POSITIONS  */ 
*scan* (f ptr j , "XI*  XI*  XI*", 

&data£03  ,&data£  1 3  , &data£23  ) ; 

*scan*  (*pzic,"Xl*  XI*  XI*  XI*" , 

StzicE0318czicE13,&zicE23,&zicC33) ; 
truad£03*data£03 ;  truad £  1 3*data£ 1 3 ; 
truad£23*data£23 i 

/*  FIRST  GET  HROW-CALCULATE  GAIN,  ESTIMATE  */ 

/*  COVARIANCE  OF  ERROR  BASED  ON  ONE  TIME  */ 

/*  MEASUREMENT-TC , TX , TY , TZ  */ 

adapti  *or  (i*Oj  i<  js;  i++) 

£ 

x*xkkml E03-xb£i 3 j 
y*xkkml C23-yb£i 3 ; 
z*xkkml£43-zb£i3; 

denom*sqrt  (x*x  J-''*y+z*z ) ; 

auxh*valm/dencitiji 

hrow£03*x*auxh; 

hrow£23*y*auxh; 

hrow£43*z*auxh; 

*or  (i i*0; i i<nmax j i i++) 

£ 

i in*i i*nmax : 


gnumEii 3*pkkml Ei in3*hrowE03+ 

pkkmlCiin+23#hrowE23+pkkmlCiin+43*hrowE43 ; 

} 

gd»nom«hr owE 03  *gnumE 03+ 
hrowE23*gnumE23+ 
hrowC43*gnumE43+rEi 3; 

/*  THIS  IS  THE  FIRST  BAIN  COLUMN  */ 

for  (ii«0|ii<nm«xjii++>  gi Ci i 3-gnumCi i 3/gdenomj 

pr intf  ("XnVECTOR  HROW  t  \n"); 
print v*c <hrow,5) ; 
printf  ("XnVECTOR  GNUM  :  \n"); 
printvsc (gnum, 5) ; 
printf  <" ^DENOMINATOR  is  »  "); 
printf <"%•  \n" , gdanom) ; 
printf ("XnVECTOR  B I  s  \n"); 
printvac (gi ,5) ;  */ 

/*  CALCULATE  THE  COVARIANCE  OF  ERROR  PI  */ 
for  (ii*0;ii<nmax;ii+a2) 

{ 

pdumEi i 3* (-gi E03*hrowEi i 3 ) ; 
pdumEii+53*(-giE13*hrowEii 3) ; 
pdumEii+103"(-gi E23*hrowEii 3) ; 
pdumEii+153a(-gi E33*hrowEi 1 3>  j 
pdumEi i +203* <-gi E43*hrowEi i 3 >  ; 

> 

pdumE03+«l. 0; 
pdumE 123+«1 . 0; 
pdumE243+«l . 0; 

pi C03  *pdumE03*pkkml E03+pdumE23*pkkml E 103+ 
pdumE43*pkkmi E203 ; 

pi E 1 3  »pdumE03*pkkml E 1 3+pdumE23*pkkml El  1 3  + 

pdumE 43*pkkml C 21 3 ; 

pi  E23  =*pdumEO]#pkkml  E23+pdumE23*pkkml  E 123  + 

pdumC43*pkkml C223 ; 

pi E33  =pdumE03*pkkml E33+pdumE2D*pkkml E 133+ 
pdumE43*pkkml C233 ; 

pi E43  »pdumE03*pkkml E43+pdumE23*pkkml E143+ 

pdumE43*pkkml C243 ; 
pi E53=*pi El]; 

pi E63  “pdumE53*pkkml E13+pkkmlE63+ 

pdumC73*pkkml Ell 3+pdumE93*pkkml E21 3 ; 
pi E73  =pdumE53*pkkml E23+pkkmlE73+ 

pdumE73*pkkml E 12D+pdumE93*pkkml E223 ; 
pi E83  =pdumE53*pkkml E33+pkkml E83+ 

pdumE73*pkkml E 133+pdumE93*pkkml E233 ; 
pi E93  =pdumE5]+pkkml E43+pkkmlE93+ 

pdumE 73*pkkm 1 E 143+pdumE93*pkkml E243 ; 
piE103*piE23; 


piC113-piC73; 

pi C 123-pdumC 103*pkkml C23+pdumC 123*pkkml C 123+ 
pdumC 143*pkkml C223 ; 

pi C 133*pdumC 103#pkkml C33+pdumC 123*pkkml C 133+ 
pdumC 143*pkkml C233 ; 

pi C 143*pdumC 103*pkkml C43+pdumC 123*pkkml C 143+ 
pdumC 143*pkkml C243  j 
piC153-piC33; 
pi C 163=pi C83 ; 
piC173-piC133; 

pi C183-pdumC153*pkkmlC33+pdumC173*pkkmlC133+ 
pkkml C 183+pdumC 193*pkkml C233 ; 
pi C 193»pdumC 153*pkkml C43+pdumC 173*pkkml C 143+ 
pkkml C 193+pdumC 193*pkkml C243 ; 
piC203«piC43j 
piC213«piC93; 
piC223-piC143j 
piC233-piC193; 

pi C243*pdumC203#pkkml C43+pdumC223*pkkml C 143+ 
pdumC243*pkkml C243 ; 

/*  CALCULATE  FIRST  MEASUREMENT  PREDICTION  */ 

zhat-velm+denom; 

zdif  f  Ci  3»zicCi  3-zhat; 

/*  COMPUTE  THE  SATE  FOR  ERRONEOUS  TIME  */ 

/*  MEASUREMENTS  */ 

pkl«<piC03  <  0)  7  -pi C03  t  piC03; 

pk3* <pi C 123  <  0)  ?  -pi C 123  :  piC123; 

pk5-(piC243  <  0)  ?  -piC243  :  piC243; 

pll»(pkl  >  pk3)  ?  pkl  :  pk3; 

p  »  (pll  >  pkS)  ?  pll  :  pk5; 

pgate=»aqrt  <p)  *velm;  */ 
x*p+r Ci 3; 
if  (x  !=  0.0) 
i 

if  (x  <  1.0)  x=1.0; 
do 

oldx*x ; 

x»(  x  +  p/x  )*0.5; 
temp*oldx-x ; 

rabs«(temp  <  0)  ?  -temp  :  temp; 

> 

while  <rabs  >  0.0); 

> 

pgate*x  +velm; 
gate=3. 0*pgate; 


/♦  printf  ("\nMATRIX  PDUM  i  \n")| 

printmat (pdum,5,5)  | 
printf (M\nMATR IX  PI  s  \n")j 
printmat (pi ,5,5) ; 
printf ("\nZDIFF  is  i 
printf ("\%s  \n"  ,zdif f Ci  3)  j 
print-f  < "\nBATE  is  i  ">; 
print-f  ("\Xs  \n",gats);  */ 

/♦EDIT  INVALID  TIME  MEASUREMENTS  ♦/ 
zgats-  (zdif  -f  Ci  I  <  0)  ?  -zdiffCi3  t  zdi-f-f  Ci3; 
if  (zgats  >■  gats) 

( 

for  (iia>0;  iiCnmax;  ii++)  giCii3=0.0; 
zdiff Ci3«999.0; 

/♦  TAS  FOR  INVALID  TIME  MEASUREMENT  ♦/ 

> 

/♦  CALCULATE  ESTIMATE  BASED  ON  ONE  MEASUREMENT  * / 

/♦  PREDICTION  ♦/ 

for  (ii*>0;  ii<nmax;  ii++) 

xi Cii 3*xkkmlCii 3+gi Cii 3*zdif f Ci 3  $ 

/♦  printf ("\nVECTOR  XI  i  \n")j 

printvec (xi ,5) ;  */ 

far  (ii»0; ii<nmax;ii++) 
f 

xkkmlCii 3»xi Cii 3 ; 
pkkmlCii 3=pi Cii 3; 

> 

for  ( jj=nmax ; j j<maxsq; j j++)  pkkmlC j j3=pi C j j 3 ; 
if  (i  ■■  3)  goto  156; 

>  /♦  end  JS  loop  ♦/ 


156: 


/♦ 

/♦ 

/♦ 

/♦ 

/♦ 

for 


NOTE  :  CALLED  ORIGINAL  X (0/-1 ) , XKKM1 .  UPDATED 
AFTER  1  MEASUREMENT  CALLED  IT  XI,  THEN  MADE 
XKKM1  AND  WENT  THRU  ITERATION  AGAIN.  AFTER 
YOU  HAVE  UPDATED  XKKM1  FOR  EACH  MEASUREMENT 
XKK=XI  AND  PKK-PI. 

(i i »0; i i <nmax ; i i ++) 


*/ 

♦/ 

*/ 

*/ 

♦/ 


*x  i  C  i  i  3 ; 
■pi  Cii  3; 


xkkCi i  3* 
pkkCi  i  3: 

> 

for  ( j j=nmax ; j jCmaxsq; j j++> 
f 

pkkC j j3»pi C j j3j 
J 


:'v.i 


;\v-m 


J 
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print* ("NnUPDATING  VECTORS  INTERVAL  ...  \n">; 

print* ("SnVECTQR  XKKM1  t  \n">; 

printvec (xkkml ,5) ; 

print* (“\nMATR IX  PKKM1  «  \n">; 

printmat (pkkmi ,5,5) ; 

print*  <"\nVECTOR  XKK  I  \n">; 
printvac <xkk,5) i 
print* <"\nMATRIX  PKK  I  \n“); 
printmat (pkk ,3,5) ;  */ 

/*  RECALCULATE  TIME  MEASUREMENTS,  and  FORM  */ 

/*  ABSOLUTE  VALUE  OF  RESIDUALS.  */ 

*or  <i*0; i<4; i++) 

/*  EDIT  INVALID  1  jlMES  FOR  ADAPTIVE  MANEUVER 
/*  ROUTINE  */ 

C 

i*  <zdi**Ci3  <  999.0) 

* 

x=xkkmlC03-xbCi3; 

y=xkkmlC23-ybCi3; 

z=xkkmlC43-zbCi3; 

/*  denom=»qrt <x#x+y*y+z*z> ;  */ 
denom=x*x+y*y+z*z ; 
x=denom; 
i*  (x  !-  0.0) 

C 

i*  < x  <  1.0)  x-1.0; 
do 

* 

oldx=x; 

x=<  x  +  denom/x  >*0.5; 
temp=oldx-x; 

rabs«(temp  <  0)  ?  -temp  :  temp; 

> 

while  (rabs  >■  l.e-12); 

> 

denom=x ; 

zhat=velm*denom; 
temp=zicCi  3-zhat; 

zdi **  Ci  3  =  (temp  <  0)  ?  -temp  :  temp; 

> 

else 

zdi**  Ci 3=0. 0; 
nzdi **-=1 ; 

> 
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/*  THIS  FUNCTION  COMPUTES  THE  ADAPTIVE  Q 
if  (i curve  ■«  1) 

C 

bb-xkkml C3D#xkkml C33 ; 
dd«xkkml C 13*xkkmlC13 ; 
al*bb+dd| 

bd«xkkml C 1 3*xkkml C33 ; 

•l“dd*sigacc/al+bb*sigcc; 

•12»bd*sigacc/al-bd*sigcc; 

•2“bb*sigacc/al+dd*sigcc; 

qC03®g2*el ; 

qC 1 3=g3*«l ; 

qC23*g2*el2; 

qC33“g3*el2; 

qC63“a2#el ; 

qC73»qC3D; 

qC83»a2*el2; 

qC12D-g2*«2; 

qC 133«g3*e2; 

qC18D-a2*»2; 

> 

•Isa 

C 

b  b  *gamma C  0  3  *g amma COD  5 
dd*gammaC 1 3*gammaC 13; 
q  C  0  3  =»bb*cavw  C  0  3 ; 
q  C 1 3 “gamma 103  *covw COD *g  amma C 1 3 ; 
qC23=0. 0;  qC33=*0.0; 
q  C  6  3  =«dd*covw  COD; 
qC7D“0.0;  qC83“0.0; 
qC 123“bb#covwC 13; 
qC 13D=gammaC0D*covwC 1 D*gammaC 13; 
qC 183“dd*covwC 1 3 ; 

> 

qC53=qC 13;  qC103»qC2D;  qCllD»qC7D; 

qC153=qC33;  qC163=qC8D;  qC 17D=qC 13D ; 


MATRIX  */ 


/*  printf ("\nMATRIX  0  s  \n”>; 

printmat (q,5,5) ;  */ 

/*  IF  ALL  TIME  MEASUREMENTS  EXCEED  GATE,  BYPASS  */ 
/*  ADAPTIVE  MANEUVER  ROUTINE.  */ 

if  (nzdiff  !«  0.0) 

< 

zdif av» (zdif f  C03+zdif f  C 13+zdiffC23+zdiffC3D) /nzdiff  ; 

/*  printf ("\nZDIFAV  is  s  ">; 

printf ("\Xe  \n",zdifav);  */ 


/#  DO  NOT  PERFORM  ADAPTIVE  MANEUVER  IF  FILTER  */ 
/*  HAS  NOT  ACHIEVED  THE  STEADY  STATE.  */ 

if  <kk  >  3) 

C 

dost i me (&i hours , &i mi ns , &i sscs , &i ths  > ; 
hours-ihours;  mins-imins;  s*cs>lsacs; 
ths-iths; 

time-3600. *hours+60. *mins+secs+0. 01*ths; 
printf ("%s  7b","  timel  -  11 , time)  $  */ 

/*  IF  ZDIFAV  MEETS  CRITERIA  TRANSFER  OUT  */ 
/*  OF  ADAPTIVE  MANEUVER  ROUTINE.  */ 

if  (zdifav  >  value  &&  icont  <  maxopt) 

{. 

/*  INCREASE  GAIN  */ 
for  ( j-0; j<maxsq ; j++) 

pkkmlC j3-pkkC j3+qC j3; 

/*  printf  <"\nINCREASE  GAIN  AND  PERFORM"); 
printf ( "  ADAPTIVE  ...  \n"); 
printf <"\nMATRIX  PKKM1  s  \n">; 
printmat (pkkml ,3,5) ;  */ 

/*  PERFORM  ADAPTIVE  MANEUVER  BY  */ 

/*  REITERATING  SAME  TIME  SLOT.  */ 
icont+-l ; 
goto  adapt; 

> 

y 

y 

icont-O; 

nzdiff-4; 

xdif f  C03-xkkC03-truedE03 ; 
xdiff  C13-xkkC23-truedC13; 
xdif  f  C23-xkkC43-truedC23 ; 

temp=xdif f  C 13*xdiffC13/pkkmlC123; 
if  (temp  <  limvar)  i curve-0 ; 
else  i curve-1 ;  */ 

f  printf  (f  pe ,  "\n  7.d  7.e  7.e  7.e",kk, 

xdiff C03, xdiff C 13, xdiff C23) ; 

/*  WRITE  */ 

printf ("\nVECTOR  ERROR  s  XDIFF  YD IFF  ZDIFFNn" ) ; 
printvec (xdif f ,3) ;  */ 

/*  PREDICTIONS  FOR  PKKM1  */ 


for  (i -0; i Cnmax ; i ++) 


phipkkCi 3»pkkCi 3+pkkCi+53*phi*C03 j 
phipkkCi+53*pkkCi+53; 

phipkkCi+103»pkkCi+103+pkkCi+lS3*phi*C23> 
phipkkCi+153«pkkCi+153 ; 
ph i p k k C i +20 2 -p k k C i +20 1 f 
} 

■for  (i  »0j  i  <nmax  |  i  ++) 

< 

i n«i*nmax | 

pktempCinl^phipkkCinl+phipkkCin+l 3*phi sCOl ; 
pkt#mpCin+l 3=phipkk[in+l 3 ; 

pktempCin+23“phipkkCin+23+phipkkCin+33*phisE23 
pktempCin+33=«phipkkCin+33 ; 
pktemp[in+43“phipkkCin+43 ; 

> 

for  ( j*Oj  j<maxsq;  j  ++)  pkkml  C  j 3=pktempC  j 3+qC  j 3 ; 

printf ( "\nPREDICTIONS  FOR  PKKM1  ...  \n“); 
printf  C'NnMATRIX  PKKM1  i  \n'*)| 
printmat (pkkml ,5,5) ;  */ 

/*  CALCULATE  NEW  XKKM1  */ 
for  (ii*0; iiCnmax; ii++) 
l 

xkkml Cii 3*xkkCii 3; 

/*  pl»sqrt (f abs (pkkC i i 3 ) ) ;  */ 

> 

xkkml C03»xkkml C03+phisC03*xkk  C 1 3 ; 
xkkml C23*xkkml C23+phi sC23*xkkC33; 

printf < "\nNEW  XKKM1  ...  (for  next  time"); 
printf ("  interval ) \n"> ; 
printf ("\n VECTOR  XKKM1  :  \n">; 
printvec (xkkml ,5) ;  */ 

f printf (fpp, "\n  Xd  Xe  '/.e  Xe" ,kk, 
pkk[03,pkk£123,pkkC243) ; 

f printf  (fpxyz  ,  "\n  m/.d  '/.e  Xe  "/.e",kk, 
xkkC03,xkk[23,xkkC43) ; 

/*  PLOTTING  */ 
col »xkkC03*x3cal e+bx ; 
row»xkkC23*yscal e+by; 
bioswd (row, col )  ; 

biosset (3,0) ; 

printf ("X*  X.  If"  ,"x  ,xkkC03>; 


biosaat (3, 13) j 

print-f  ("Xa  X.  l-f","y  -'',xkkC2D)» 
biosaat (3,25) j 

print-f  <**Xs  X.i-f","z  ■  '*,xkkC43>j 
biosaat (3,42) ; 

print-f  ("Xa  X.  1-f"  ,  "vx  -“,xkkC13) 
biosaat (3,54) ; 

print-f  ("Xa  X.  lf","vy  »“,xkkC3D) 
biosaat (3,65) j 

print-f  ( "Xa  Xd'V'tima  »'',kk)| 

>  /*  and  TIME  loop  #/ 

-f closa (-f pxyz )  ; 
f close(-fpa)  ; 

-fclosa(fpp) ; 
f closa(f pzic) j 
f closa(fptr j) | 
c»kayrd  ( ) ; 
biosini (3) ; 

print-f  ("\nquit  ?  CY/N3  i")  j 
scan-f  ( "Xs"  ,&c)  ; 

i-f  (c»«'n*  !!  c== ' N ' )  goto  rastart; 


>  /*  and  MAIN  PROGRAM  */ 


/a####*#####################*###########***###*##*#####*#*#/ 
/■*  */ 

/*  FRAME  */ 

/*  Draw  a  frame  on  the  acrean.  */ 

/*  **•*  NOTE  ***  #/ 

/*  Tha  uppar  left  cornar  of  tha  acraan  is  0,0  and  tha  */ 

/*  bottom  right  cornar  ia  199,639.  */ 

/*  */ 

/*  */ 

/***«*«*****«««#«*#***###«*#*##*#*###***##**#*«**#**#*#**#*/ 

int  f rama (ulrow, ulcol ,lrrow,lrcol > 
int  ulrow, ulcol ,lrrow,lrcol j 
i 

i  nt  count , i row , i , i aux , i col , tcount ; 


/*  do  top  lina  -first  •*/ 

-for  (icol  »  ulcol;  icol 
bioswd (ulrow, icol ) ; 


lrcol;  icol++) 


/*  do  both  si das  at  once  */ 

-for  (irow  ■  ulrow+l;  irow  <  lrrow;  irow++) 
{ 

bioswd (irow, ulcol ) ; 
bioswd (irow, lrcol ) ; 

> 


/*  do  bottom  line  -*/ 

for  (icol  *  ulcol;  icol 

bioswd (lrrow, icol ) ; 


lrcol;  icol++) 


/*  do  vertical  center  line  with  scale  */ 
i  *0. 5* (ulcol+lrcol ) +0. 5; 

for  (irow  ■  ulrow+l;  irow  <  lrrow;  irow++>  bioswd (irow, i ) ; 
for  (irow  =»  ulrow+16;  irow  <  lrrow;  irow+=»16) 

for  (tcount=i-4;  tcount  <=  i+4;  tcount++) 

bi oswd ( i row , tcount ) ; 

/•*  do  horizontal  center  line  with  scale  */ 
i  =0. 5* (ulrow+l rrow) +0. 5; 

for  (icol  =  ulcol+1;  icol  <  lrcol;  icol++>  bioswd (i , icol ) ; 
for  (icol  =  ul col +60;  icol  <  lrcol;  icol+a60> 

for  (tcount=i-3;  tcount  <=  i+3;  tcount++) 

bioswd (tcount, icol > ; 


return(O):  /*  done  */ 


/#  #/ 

/*  TRAJECTORY  SIMULATION  ROUTINE  -  filet  TR.C  */ 

/*  */ 

/*##**##*#*#*#**■*#*•***•»***#**#******■***■»•»■*<*■»**•**■*•***■»■*•*•*•»•»*/ 

#include  "stdio.h" 
tinclude  "math.h" 

^include  "kalman.h" 

trajec (narray, hydro, xb , yb , zb ,x kkml , j start , jend, wd,g, worg) 
double  hydro! 3 ,xbC3 ,ybE3 , zb! 3 , xkkml C  3 , wd ,g; 
int  narray,worg, jstart , jend; 
i 

int  pli,i, start , end ; 

double  ppl,rl,sqrt O ,zcs,zicC43,ziC43; 
double  xd,yd,zd,v,den,velm,gg$ 

static  double  xnC63*{-3. 01 ,  -2.0,  -1.5,  1.0,  -0.6, 

0. 0> ; 

static  double  ynC63*CO.O,  0.0228,  0.0668,  0.1357, 

0.2743,  0. 53 ; 

static  double  snC53-{43. 8596,  11.3636,  7.25689, 

2.891352,  2. 658875 ; 

double  data!53,wr; 
int  runn,i8; 

FILE  *fptrj,  #fopen(); 

FILE  *fpzic,  *fopen<); 
int  j,jj; 

/*  THIS  ROUTINE  COMPUTES  TRUE  TRAJECTORY  OF  TORPEDO 
dataC03»true  x  position,  data!13=  true  y  position 
dataC23*true  z  position,  w*turn  rate 
dataC33»x_yeloci ty ,  dataC43=y_veloci ty 
jstart*time  slot  where  maneuver  starts 

jend*time  slot  where  maneuver  ends  */ 

dataC03*x kkml C03 ;  data! 1 3=x kkml 121 ;  data! 2 3 -xkkml C43 ; 
dataC33*xkkml  C  1  3 ;  dataC4  3=xkkml  C33 ;  v#lm*l .  0/vel ; 
r 1*0. 49; 

wr=twopi*wd/360. 0;  gg=32.2*g;  start=jstart ;  end»jend; 

printf("\n  Y.s  \n",  "start  execution  of  trj.c"); 
f ptr j*f open ( "tr j . dat " , "w" > ; 
fpzic=fopen ("zic.dat" , "w"> ; 

for  ( j j*0; j j<npt; j j++> 

< 

/*  GET  HYDROPHONE  ARRAY  COORDINATES  */ 
if  (narray  ! =  0) 


{ 

if  (dataC03  >  awO)  i8-0| 

•las  if  <dataC03  >  awl)  iB«12j 

ala*  if  <data£03  >  aw2)  i8-24j 

alaa  if  (data£03  >  aw3)  iB-36| 

alaa  if  (data£03  >  aw4)  i8«48j 

alaa  i 8“60f 
for  <i-0|i<4ji++) 
i 

j-3*i+i8; 
xbCi 3»hydroC j3; 
ybCi 3«hydroE j+13j 
zbCi 3»hydroC j+23 ; 

y 

y 

/*  COMPUTE  THE  TRUE  TIMES  AND  THE  TRUE  POSITIONS 
WRITE  TRUE  TIME  and  TRUE  POSITIONS  */ 
printf  ("Xa",".  ">  H 
fprintf  <fptr  j  ,  "\n  V.a  Xa  Xa" , 

data£03 , dataC13 ,  dataC23) ; 

for  (i*0|  i<4|  i++> 

£ 

xd-dataCOD-xbCi 1 j 

yd*c»at  at  1 3-yb  C  i  1 ; 

zd-d*taC23-zb£i3; 

den*sqrt (xd*xd+yd*yd+zd*zd) ; 

zi£i3  aval  m*den ; 

/*  printf  C"\nTRUE  TIMES  i  \n">; 

printvec (zi ,4) ; 

printf <"\nTRUE  POSITIONS  s  \n">| 
printvec (data, 3) ;  */ 

/*  GET  NOISE, SCALE  AND  ADD  TO  TRUE  */ 

/*  MEASUREMENT  TIME  */ 

ppl=317. 0*r 1 ; 

pliappl; 

rl-ppl-pli j 

v=rl; 

j»0; 

if  <v  >  0.5)  v= 1 . 0— r 1 ; 
r2i  if  <v  <  ynEj+13)  goto  rSj 
j+“l  5 
goto  r2; 

r8s  v*»  ( v-ynE  j  3 )  *sn£  j  3+xnC  j  3  ; 
if  (rl  >®  0.5)  va(-v); 
zcs* (v/3. 00) *0. 00001 ; 

/*  printf  ("\nRAND0M  NUMBER  s"); 

printf ("Xe  \n",zcs) 5*/ 
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zicCi 3«zcs+zi Ci 3 > 

> 

fprintf  (fpzic, "\n  %e  %e  ’Am  %e", 

zicC03,zicC13,zicC23,zicC33) > 

/*  NEW  DATA  */ 

if  (start  >-  npt)  line< jj ,0, npt , data) ; 

•Is* 

{ 

if  (end  >  npt)  end*npt; 

lins( j j ,0, start ,data) ; 

ci r cl s  <  j  j , start , and , data , wr , gg , worg  > ; 

lina( j j , end, npt, data) ; 

> 

> 

f close (f p -r j ) j 
f closa(f pzic) ; 

> 


circle < j, start, and, data, ww,gg, worg) 
double  dataCl ,ww,gg; 
int  j ,st art , end, worg j 
< 

double  si n  < ) , cos ( ) , at an ( ) ; 

double  r , vt ,aux ,a,b , teta, tetaO,m,alf a, wa,ga,g , 
0*90)  w-wwj 

if(j  >■  start  liit  j  <  end) 

if  (dataC33«0.0  &&  dataC43«-0. 0)  goto  out; 
vt-sqrt  <dataC33*dataC33+dataC4D*dataC43) ; 
if  (worg«*l)  g=w*vt; 
else  w*g/vt; 
r»vt/w; 

if  <dataC33— 0.0) 
i 

b  "dated  3; 
if  (dataC43  >0. 0) 
l 

a-dataCO 3-r; 
teta0«0. 0; 

> 

else 

< 

a«dataC03+r; 
eta0-0. 5*twopi ; 

> 

> 

else  if  (dataC43**0. 0) 


•■data CO 3 | 
if  (dat«C33>0.0) 

{ 

b«dataC13+r; 
teta0--0. 25*twopi  ; 

> 

•Isa 

b«dataC13-rj 
teta0“0. 25*twopi ; 

> 

> 

•Is* 

{ 

m-data[4]/dataC33 | 
aux*r/sqrt  < 1 . 0+m*m> ; 
if  (dataC33  >  0.0)  aux=*-aux; 
b»dataC13-aux| 
a»dataC03+m* (dataC 1 3-b) ; 
teta0*atan2 <-dataC33 ,dataC43 ) ; 

> 

tata»l . 31*w+teta0; 
dataC 03 *r*cos (t*ta) +aj 
dataC 1 3*r*sin (tata) +bj 
dataC33“-vt*sin (tata) ; 
dataC43“vt*cos (tata) ; 
if  <j»*end-l) 

{ 

wa=w*360. 0/twopi ; 
ga*g/32.2; 

printf <"\n  ...  from  TRAJEC  :  "); 

printf  < "\n" ) ; 

printf  <"\n)4s  5Ce  54s","  turn  rata  :  ",wa, 

11  dg/sag"); 

printf  < "\n54s  %.2f\n" ,"  g-number  :  ",ga>; 

> 

out  a 

> 


1 i ne  <  j , start , and , dat a  > 
double  dataC 1; 
int  j, start, end j 
C 

if((j  >»  start)  &&  <j  <  e 

C 

dataC03+» 1 . 31*dataC33 ; 
dataC 1  3+=l . 31*dataC43 ; 
> 

J 


end) ) 


m 
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APPENDIX  F 


LISTING  OF  CIEKR1.C  -  CIE  VERSION 
/##«#**«-»**#*##«#######*#####**#*#**###*##*#****##*#*#**##*/ 


/*  */ 

/*  HEADER  FILE  -  KALMAN. H  */ 

/*  CIE  MICROCOMPUTER  VERSION  */ 

/*  */ 


/****«*********###*#*«#*#***********«************#*******#** 

#def ine  nmax  S  /*  #  of  col.  in  pkkv  q  ...  */ 

#dafine  maxsq  25  /#  aiza  o-f  aquiv.  vactor  PKK,Q...*/ 

#da-fina  js  4  /*  #  hydroph.  in  any  hydrophone  */ 

/*  array  */ 

#defina  hcmax  12  /*  #  of  col.  in  matrix  HYDRO  o-f  */ 

/*  hydroph.  coord.  */ 

#de-fina  hmax  72  /*  aquiv.  vactor  aiza  of  HYDRO  */ 

#define  dmax  3 

#define  itime  150  /*  max.#  of  tima  slots  */ 

#dafina  npt  150 

#dafine  icmax  5  /*  max.#  of  optimal  iterations  */ 

/#  allowed  */ 

#define  criter  0.1  /*  criteria  to  compare  ZDIFAV  */ 

#define  na  0  /*  may  assume  ONLY  values  0  or  6  */ 

/*  0  =>single$ 6*>  multiple  arrays  */ 
#define  xpoa  10000.0  /*  initial  torpedo  x-coordinate  */ 

#define  xvel  -50.0  /*  initial  torpedo  x-velocity  */ 

#define  ypos  0.0  /*  initial  torpedo  y-position  */ 

#define  yvel  5.0  /*  initial  torpedo  y-velocity  */ 

#define  zpos  0.0  /*  initial  torpedo  z-position  */ 


#define  swO  33000.0  /*  swi  define  paints  where  */ 

#define  swl  27000.0  /•*  switching  from  one  to  */ 

#define  sw2  21000.0  /*  another  */ 

#define  sw3  15000.0  /*  array  */ 

#define  sw4  9000.0  /#  is  done.  */ 

#define  sigdiv  1.0 

#define  see  0.387463094  /*  -  in  radians...  */ 

#define  sacc  36.2 

#define  vel  4860.0  /*  speed  of  sound  */ 

#define  twopi  6.28315308 

#define  rrvar  l.e-5 

#define  rvar  l.e-10 

#define  pkkdiag  1000.0 


#define  limvar  1.5  /*  defines  threshold  to  use  Q*/ 

/*  constant  */ 

#define  gmO  0.86 
#define  gml  1.31 
#def i ne  cwO  1 . 0 
#define  cwl  1.0 
#define  cw2  1.0 


/*  initial  was  1036.84  */ 
/*  initial  was  1036.84  */ 


/a*********************************************************/ 
/*  */ 
/*  KALMAN  FILTER  PROGRAM  FOR  THE  CIE  MICRO  */ 
/*  (FILE  CIEKR1.C)  */ 
/*  */ 


/***4HHHHH»***#*#****#******#***#*#*******#**********#*#**-»*/ 

(♦include  "stdio.h" 

(♦include  "math.h" 

#include  "kalman.h" 
double 

xkkmlCnmaxl ,pkkml Cmaxsq3 ,pkkCmaxsq3 ,xkkCnmax3 , dataCdmax 3; 

/*  STARTING  THE  MAIN  PROGRAM. . .  */ 

mai n ( ) 

{ 

FILE  *f pxyz ,  *fopen(); 

FILE  *fpe,  *fopen(); 

FILE  *fpp,  *fopen<); 

FILE  *fptrj,  *fopen(>; 

FILE  *fpzic,  *fopen  ()  ; 

FILE  *fparam,  *fopen<); 

FILE  *fpin,  *f open O ; 

int  i hours , i mi ns ,  i secs ,  iths, col , row; 

int  iS,i , j ,kk,iin,icont,i curve 'Option, par am; 

int  tr jopt ,nrange; 

int  ii  ,  j  j  , narray ,nzdif f , in ,pli  ,runn ,  c; 
int  j start , j end, war g; 
float  rabs,zgate; 

doub  1  e  height, width,  ymax  ,  ymi  n ,  x  max  ,  x  mi  n ; 
double  xscale,yscale,bx ,by; 

doubl e  hours ,mins, secs , ths , t i me , ol dt i me , del  tat ; 

double  sqrt () ,oldx , value; 

daub 1 e  denam ,al4,gdenam,gate,zdifav; 

double  den, velm, temp; 

double  a2,gl ,g2,g3,pl ,xdiff C33; 

double  zhat,zcs,dk; 

double  v, trued C 33 ,wd,g; 

double  x ,y,z ,auxh,xd,yd,zd ,sigcc,sigacc; 

double  al ,el ,el2,e2,bb,dd,bd; 

double  pkl ,pk3,pk5,pgate, rgate,p ,pl 1 ; 

double  phipkkCmaxsql , pktemp Cmaxsq 3 , q Cmaxsq 3 ; 

double  pi Cmaxsq 3 ,pdum Cmaxsq 3 , qtemp Cmaxsq 3 ; 

double  gnumCnmax 3 , gi Cnmax 3 ,xi Cnmax 3 , hrowCnmax 3 ; 

double  zicCjs3,ziCjs3,zdiffCjs3,rCjs3,phisCjs3,ztCjs3; 

double  xbCjs3,ybCjs3,zbCjs3,rrCjs3; 

double  hydroChmax3 ,covwC33 ,gammaC23 ,optC53; 

f pxyz»f open ( "xkk. dat " , "w" ) ; 
f pe*f open ( "xdif f . dat " , "w" ) ; 

f  pp=»f  open  <  "pkk.  dat"  ,  "w"  > ; 
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/*  INITIALIZE  CONSTANTS,  HYDROPHONE  MATRIX,  PHI,  R, 
DATA  FOR  TRAJECTORY,  AND  LOAD  X<0/-1>,  PCO/-1) 

r»zdiff=4;  icont=0;  icurva=l;  value=cri tar ;  wd=5.730; 
runn«l;  narray=na;  param=0;  worg=l;  g=0. 156; 

nranga=0; 

jstart=40;  jend=80; 

hydro!03*36000. 0;  hydro! 13=  6000.0;  hydro!23=  0.0; 

hydro!33=36030. 0;  hydro!43=  6000.0;  hydro!53=  0.0; 

hydro!63=36000.0;  hydro!73=  6030.0;  hydro!B3=  0.0; 

hydro! 93 *36000. 0;  hydro! 103*6000. 0;  hydro! 11 3*  30.0; 

hydro! 123=30000.0;  hydro! 133=6000. 0;  hydro! 143=  0.0; 

hydro! 153=30030.0;  hydro! 163=6000. 0;  hydro! 173=  0.0; 

hydro! 183=30000. 0;  hydro! 193=6030. 0;  hydro!203=  0.0; 
hydro!21 3=30000.0;  hydro!223=6000. 0;  hydro!233-  30.0; 

hydro!243=24000. 0;  hydro! 25 3 =6000. 0;  hydro!263=  0.0; 
hydro I 27 3 =24030.0;  hydro!2B3=6000. 0;  hydro!293=  0.0; 
hydro I 30 3 =24000. 0;  hydro!31 3=6030. 0;  hydro!323=  0.0; 
hydro I 33 3 =24000. 0;  hydro!343=6000. 0;  hydro!353=  30.0; 

hydro!363 =18000. 0;  hydro!373=6000. 0;  hydro!3B3=  0.0; 
hydro!393=18030. 0;  hydro!403=6000. 0;  hydro!413»  0.0; 
hydro!423=18000. 0;  hydro! 43 3 =6030. 0;  hydro!443=  0.0; 
hydro!453=18000. 0;  hydro!463=6000. 0;  hydro!473=  30.0; 

hydro 1483 =12000. 0;  hydro!493»6000. 0;  hydro!503=  0.0; 
hydroISl 3=12030.0;  hydro! 52 3 =6000. 0;  hydro!533=  0.0; 
hydro!543=12000. 0;  hydro! 55 3 =6030. 0;  hydro!563=  0.0; 
hydra!573=12000. 0;  hydro! 58 3 =6000. 0;  hydro!593=  30.0; 

hydro!603=6000. 0;  hydro!61 3=6000. 0;  hydro!623=  0.0; 
hydro!633=6030. 0;  hydro!643=6000. 0;  hydro!653=  0.0; 

hydro!663=6000. 0;  hydro!673=6030. 0;  hydro!683=  0.0; 

hydro!693=6000. 0;  hydro!703=6000. 0;  hydro!713=  30.0; 

r!03=rvar;  r!13=rvar;  r!23=rvar;  r!33=rvar; 

gamma! 03 =gmO;  gamma! 1 3=gml ; 
covw!03=cw0;  covw!13=cwl;  covw!23=cw2; 

xkkml! 03*1000. 0;  xkkml ! 1 3* <-50. 0) ;  xkkml !23=0. 0; 
xkkml !33=5. 0;  xkkml !43=0. 0; 


for  <i=0; i <nmax ; i++) 

for  < j=0; j<nmax ; j++)  pkkml  !i*r»max+j  3=0. 0; 
for  <i=0; i<nmax; i++)  pkkml !i *nmax+i 3=pkkdi ag; 


phi s!03=l . 31 ;  phis!13=0.0;  phis!23=l . 31 ;  phis!33=0.0; 
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xbC03«(-15.0) |  ybC03-(-lS.0> i  zbC03-(-15.0> ; 


xbCll-  15.0; 
xbC23»(-15.0) ; 


ybC13*(-15.0) ;  zbC 1 3- (-15. 0) ; 


ybC23«  15.0; 


zbC23=(-iS.O> ; 


xbC3I-(-15.0) ;  ybC33» (-15. 0) ;  zbC33-  15.0; 

/*  GET  DATA  INPUT  */ 

/a****##*##**##*#****#*#*#**####*##*#***#**#***#*********#*/ 
/*  */ 

/*  DATA  INPUT  */ 

/*  */ 


menu: 


/*  OPTIONS  MENU  */ 
printf ("\n") ; 
print-f  ( "\n 
print-f  ("Nn”)  ; 


KALMAN  FILTER  ">; 


1)  RUN  TRAJECTORY"); 

2)  RUN  KALMAN"); 

3)  MODIFY  PARAMETERS"); 

4)  QUIT"); 


print-f  <"\n  OPTIONS  :">; 

printf  <"\n" ) ; 

print-f  <"\n  1)  RUN  TRAJECTORY" )  ; 

printf  <"\n") ; 

printf <"\n  2)  RUN  KALMAN"); 

printf ("\n") ; 

printf <"\n  3)  MODIFY  PARAMETERS"); 

printf  <"\n") ; 

printf  ("\n  4)  QUIT"); 

printf  <"\n") ; 
if  (param!=0) 

printf ("\n") ; 

printf ("NnA  parameter  was  changed  ...  ">; 

printf <"  You  may  have  to  run"); 

printf <"  TRAJECTORY  before  continue."); 

} 

printf < "\nEnter  value  (1-4)  :  "); 
scanf  ( "'/.ct "  , ^option)  ; 
if  (option**!) 

< 

fpin*fopen ("tr j. inp", "w") ; 
f printf (f pin , "Xd  Xd"  ,narray,runn)  ; 
f printf (f pin, "\nXe  Xe  '/.e", 

xkkmlCOD,xkkmlC13,xkkmlC21> ; 
f  printf  (fpin,  "NnXe  Xe" ,xkkml C3D ,xkkml C43 )  ; 
f close(fpin) ; 


I 


tra jec (narray ,hydro,xb ,yb , zb , 
param*0; 
goto  menu; 

> 


xkkml , j start , jend , wd ,g , worg) ; 


PS 
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■Isa  if  (option—2)  goto  run} 

■Isa  if  (option«3)  goto  modify; 

•Isa  if  (option«*4)  goto  quit; 

•Isa  goto  manu; 

modify: 

printf ( "\n  MODIFY  PARAMETERS  OPTIONS  s "); 

printf  <"\n") ; 

printf  ("\n  1)  ZDIFAV  stop  optimization"); 

printf ("critaria" ) ; 

printf  < "\n  2)  SINGLE  /  MULTIPLE  ARRAY"); 

printf <"\n  3)  INITIAL  X,  Yf  and  Z  positions"); 

printf ("\n  4)  INITIAL  Vx  and  Vy  velocities"); 

printf ("\n  5)  NOISE  COVARIANCE  -  diag_elem  of") 

printf ("R"> ; 

printf ("\n  6)  ERROR  COVARIANCE  -  diag_elem  of") 

printf <“  PKK") ; 
printf ("\n") ; 

printf <"\n  7)  MAIN  MENU  -  OPTIONS"); 

printf  <"\n") ; 

printf < "\nEnter  value  <l-7)  s  " ); 
scanf  ( "  '/.d  "  ,&param) ; 

swi tch (par am) 

casa  1: 

printf  <"\n  ‘/.s  Xe% 

"Current  CRITERIA  is  value); 
printf  ("\nchange  ?  CY/N3  :"); 
scanf ( "%s" ,&c) ; 
if  (C»='n'  ii  c=«'N'>  break; 
printf ("\nEnter  new  value  (real)  :  "); 
scanf ("Xlf" ,& value) ; 
break; 

case  2: 

printf ( "\nSINGLE  array  is  the  default"); 
if  (narray»0) 

printf ("\nXs" , "Current  case  is  :  SINGLE"); 
else 

printf < "\n%s" , "Current  case  is  :  MULTIPLE"); 
printf ( "\nchange  ?  CY/N3  :"); 
scanf ( "%s" , &c) ; 
if  (c»»'n'  !!  c=='N')  break; 
printf < "\nEnter  S  for  SINGLE  or"); 
printf ("  M  for  MULTIPLE  array  :  "); 
scanf  ("7.s",&c)  ; 

if  (c»'s*  II  c®='S')  narray=»0; 
else  if  (c=='m'  !!  c=*='M')  narray=l; 
else  narray=»0; 
break: 


casa  3: 

printf ( "NnCurrent  values  ara  s  "); 
printf  <"\n7s  7e  7s  Xe  %s  7e","x poa  ■  '*  ,xkkmlC03 
"  ypos  =  " ,xkkml C23 , "  zpos  =*  ",xkkmlC43) 
printf ("\nchanga  ?  CY/N3 
scanf < "7s" ,&c)  ; 
if  <c~'n'  I!  c«'N'>  break; 
printf  ("\nEntar  new  valuas  t 
printf <"\n7s" , "xpos  ■  ">; 
scanf  < "%lf " ,&xkkml C03 ) ; 

printf  <"\n7s" , "ypos  ■  ">! 
scanf ("Xlf " ,&xkkmlC23> ; 

printf <"\n7s"f"zpos  -  ">; 
scanf ( "%lf " , &xkkml E43  > ; 

break; 

case  4: 

printf < "\nCurrent  values  are  :  " ); 
printf <"\n7s  %e  V. s  Xe" ,"xve  1  -  " ,xkkml C 1 3 , 

"  yvel  ■  " ,  xkkml E33 ) ; 
printf ( "Nnchange  ?  CY/N3 
scanf  <  "7 s"  ,&c) ; 
if  (ca«*'n'  >!  ca*'N')  break; 
printf < "\nEnter  new  values  :  " ); 
printf  <"\nXs","xvel  - 
scanf  ( "Xlf  "  ,  Scxkkml  C 1 3  > ; 

printf < "NnXs" , "yvel  ■  ">; 
scanf  ("‘/.If  ",&xkkmlC33> ; 

break; 

case  5: 

printf < "\nCur rent  values  are  : 
printf <”\nXs  Y.e  Y.s  Y.e  V. s  Y.e  X s  7e","rC03  =  ", 
rCOl,"  rCl]  »  "  ,  r  C  1 3  ,  "  rC23  *  ",rC23, 

"  r  C33  =  "  ,  r  E  3  3 ) ; 
printf ( "Nnchange  ?  EY/N3 
scanf ( "7s" ,&c) ; 
if  <c=='n'  ! !  c== 'N ' )  break; 

printf ( "\nEnter  new  values  s  ">; 
printf ("NnXs ","rE 03  =  "); 
scanf  ("Xlf  ",3<rE03> ; 

printf ("NnXs", "rE 13  =  ">; 
scanf  ( "71 f ", &rE  1  3 ) ; 

printf ("NnXs" , "rE23  «  " ) : 


scanf  <  "Xlf  "  ,8«r  E23 )  | 


printf ("\nXs","rE33  -  ">; 
scanf  ("Xlf  ”,8<rE33)  | 

break  j 

CM 6f 

printf ( "\nCur rent  values  are  3  ">; 
printf ( "NnXs  Xe  Xs  Xe  Xs  Xe" , "pkkml E03  -  ", 
pkkmlE03,"  pkkml E63  *  ",pkkmlE63, 

"  pkkml C 121  -  ",pkkmlE123> ; 
printf ("NnXs  Xe  Xs  Xe" , "pkkml E 183  *  ", pkkml E 183 
"  pkkml E243  »  ", pkkml E243) ; 
printf ( "Nnchange  ?  EY/N3  3" >; 
scanf ("Xs" ,&c> ; 
if  (c~'n'  !S  c-«'N'>  break; 
printf < "\nEn ter  new  values  3  "); 
printf ("NnXs" , "pkkml E03  ■ 
scanf  CXI f " ,&pkkffllC03 ) ; 

printf ("NnXs" , "pkkml C63  -  ">; 
scanf  ("Xlf  "  , &pkkmlE63)  ; 

printf <"\nXs" , "pkkml E 123  =  ">; 
scanf ("Xlf ",&pkkmlC 123) ; 

printf ("NnXs", "pkkml E 183  -  "); 
scanf  ( "XI  f  "  ,  Scpkkml  E 183 ) ; 

printf ("NnXs" ,"pkkmlE243  = 
scanf  ("Xlf  ",8<pkkmlE243> ; 

break; 

case  7s 

goto  menu; 
break; 

default  3 
break; 

> 

goto  modify; 

/*  START  EXECUTION  */ 
printf  ("\n") ; 

printf ( "\nEnter  run  #  s  "); 
scanf ("Xd" ,&runn) ; 

f par am»f open ( "param. dat" , "w" ) ; 


fprintf  (fpxyz  ,  "X2d"  ,runn) ; 
fprintf  (f  pe,  "X2d"  ,runn)  ; 
fprintf  (fpp, "X2d"  ,runn> ; 
fprintf  (f param,  "X2d"  ,runn)  ; 

if  (narray»0) 

{ 

fprintf (fpxyz , "Xs" , "  Singla") ; 
fprintf  (fpet"*/.s","  Singla") ; 
fprintf  <f pp , "Xs" , "  Single") ; 
fprintf  (f param,  "Xs"  ,  "  Single") ; 

> 

else 

l 

fprintf  (fpxyz  ,  "*/.s"  ,  "  Mult") ; 
fprintf  (fpe,  "Xs"  ,  "  Mult"); 
fprintf (fpp , "Xs" , "  Mult"); 
fprintf (f param, "%s", "  Mult") ; 

>  */ 

fprintf (f param, "\n  Xs  Xe" , "CRITERIA  =  value); 
fprintf (f param, "\nXs  Xe  X  s  Xe  Xs  Xe"  ,  "xkkml  C03  =  ", 

xkkmlC03,"  xkkml C 1  3  -  ",xkkmlC13, 

"  xkkml C23  «  ",xkkmlC23); 

fprintf (f param, "\nXs  Xe  Xs  Xe" , "xkkml C33  «  ", xkkml C33 
"  xkkml C43  ■  ",xkkmlC43); 
fprintf (f param, "\nXs  Xe  Xs  Xe  Xs  Xe  Xs  Xe", 

"r CO]  -  " , r C 0 3 , "  rC13  *  ",rC13, 

"  r C23  -  " ,r C23 , "  rC33  ■  M,rC33); 
f printf (f param, "\nXs  Xe  Xs  Xe  Xs  Xe" , "pkkml C03  =  ", 
pkkml C03 , "  pkkml Cl  3  ■  ",pkkmlC13, 

"  pkkml C23  -  ",pkkmlC23); 
f close (f param) ; 

f pin»f open ( "tr j . inp" , "w" > ; 

fprintf (fpin, "Xd  Xd" ,narray,runn) ; 
fprintf (f pin , "NnXe  Xe  Xe", 

xkkmlC03,xkkmlC23,xkkmlC43) ; /^positions*/ 
fprintf (fpin , "\nXe  Xe", 

xkkmlC13,xkkmlC33) ; /*velocities#/ 
f close (fpin) ; 
if  <option«4)  goto  quit; 

f ptr j=f open ( "tr j . dat " , "r" > ; 
f  pzic=»f  open  ( "zi  c.  dat "  ,  "r  " ) ; 

/*  INITIALIZATION  FOR  QFIND  */ 
vel m*l . O/vel ; 
al4=phi sC03 ; 
a2*al4*al4; 


gl»a2*0.5; 

g2-gl*gl; 

g3»«14*gl; 

si gacc«sacc*sacc ; 

si gcc«scc*scc  j 

for  <i“Oj iCnmax ; i++) 

for  < j*0; j<nmax ; j++>  qCi*nmax+j3«0. 0; 
qCmaxsq-1 3*sigdi v*a2*sigdi vj 

/#  INITIALIZATION  FOR  HRQW  */ 
hrowC13**0.  0;  hrowC33*0.0; 

/*  INITIALIZATION  FOR  PDUM  */ 
for  (ii*0; ii<nmax; ii++> 

< 

j j«ii*nmax) 
pdumC j j+1 3»0. 0; 
pdumC j j+33=0. 0; 

> 

pdumC63  =  1.0;  pdumC  183=*1 . 0; 

/*  INITIALIZING  RR  */ 

for  <i  i=0;  i  i<4;  i  i++)  rr Ci i 3=sqrt <r Ci i 1 ) ; 


pr intf < "\nPress  any  key  and  ENTER  to  start  ... 
scanf  ("•/.s,,,&c> ; 


/*  INSERT  TIME  CHECKING  HERE  */ 

/*  START  THE  TIME  SLOT  LOOP  AND  SET  ARRAY  HANDOFF  */ 
/*  POINT  */ 

for  <kk=Oj kk<itime; kk++) 
i 

printf  < "\n7.s  Xe"  ,"t  =  ", oldtime); 
printf  <"*/.s  7.d"  kk  ■  "  ,kk);  */ 

dk=*kk+l ; 

printf  < "\n**************#********************" ) ;■»/ 
printf < "%20s  5£d  \n "," **■»*■»**•*■»****  TIME  :",kk);  */ 
printf ( "\nwait  "); 
printf  ; 

/*  GET  HYDROPHONE  ARRAY  COORDINATES  */ 
if  (narray  ! =  0) 

•C 

if  (xkkmlC03  >  swO)  i8=0; 
else  if  (xkkmlCOl  >  swl)  i8=*12; 

else  if  (xkkmlC03  >  sw2)  i8=24; 

else  if  (xkkmlCOl  >  sw3)  i8=»36; 

else  if  <xkkmlC03  >  sw4)  i8=48; 


•Isa  i 8*60 | 

■for  <i«0|  i<4;  i++) 

{ 

j»3*i+i8; 
xbCi  3*hydroC  j3 ; 
ybCi 3«hydroC j+13 j 
zbCi 3-hydroC j+23 ; 

} 

> 

/*  INSERT  TIME  CHCKING  +  WAITING  LOOP  HERE  */ 


gettp: 

/*  GET  THE  TRUE  TIMES  AND  THE  TRUE  POSITIONS 
WRITE  TRUE  TIME  and  TRUE  POSITIONS  */ 
f scanf  (f ptr j  ,  UX1  f  XI *  XI  f  “ , 

&dataC03  ,  StdataC  1 3  , &dataC23  > ; 
fscan-f  (fpzicv"Xlf  XI  f  XI  f  XI  t"  f 

&zicC03,8czicC13,8.2icC23,&zicC33)  ; 
truedC03adataC03 ;  truedC 1 3=dataC 13; 
truedC23=dataC23 ; 

/*  FIRST  GET  HROW-CALCULATE  GAIN, ESTIMATE  */ 
/*  COVARIANCE  OF  ERROR  BASED  ON  ONE  TIME  */ 
/*  MEASUREMENT-TC , TX , TY , TZ  */ 

adapt:  -for  <i*0;  i<  js;  i++) 

{ 

x=xkkml C03-xbCi 3; 
yaxkkml C23-ybCi 3 ; 
zaxkkml C43-zbCi 3; 

denomasqrt  <x*x+y*y+z*z  >  ; 
aux  h=vel m/denom ; 
hrowC03=x*auxh j 
hrowC23=y*auxh; 
hrowC43=»z*auxh; 

■for  (i i=0;  i  i<nmax ;  i i  ++> 

i inai i*nmax ; 

gnumCi i 3=pkkml Ti in3*hrowC03+ 

pkkml Ci in+23*hrowC23+pkkml Ciin+43*hrowC43 ; 

> 


ienonv 


BhrowC03*gnumC03+hrowC23*gnumC23+hrowC43*gnu 

/*  THIS  IS  THE  FIRST  GAIN  COLUMN  */ 
for  (iiaO;  iiCnmax  ;  ii++>  gi  Ci i  3=gnumCi i 


umC43+r Ci 3 


3/gdenom; 


printf ("\nVECTOR  HROW  :  \n">; 


printv«c (brow, 5> ; 

print-f  ("\n VECTOR  GNUM  *  \n">; 

printvmc (gnum,5) ; 


print-f  < "NnDENOMINATQR  is  i  ">; 
print-f  ("%■  \n"  ,  gdanom) ; 
print-f  ( "\nVECTDR  GI  t  \n"); 
printvec (gi ,5) ;  */ 

/*  CALCULATE  THE  COVARIANCE  OF  ERROR  PI  */ 

■for  (ii-O; ii<nmax; ii+«2) 

{ 

pdumCii I* (-gi [03*hrowCi i 3 ) ; 
pdumCi i +53- (-gi C 1 3+hroffCi i 3 ) ; 
pdumCi i+103* <-gi C23*hrowCii 3) ; 
pdumCi i +153- (-gi C33+hrowCii 3) ; 
pdumCi i +203“ (-gi C43*hrowCi i 3 ) ; 

> 

pdumC03+-l .  0;. 
pdumC  123+-1 .0; 
pdumC243+»l . 0; 

pi C03  -pdumC03*pkkml C03+pdumC23*pkkml C 103+ 
pdumC43*pkkml C203 ; 

pi C 1 3  =pdumC03+pkkml C 1 3+pdumC23*pkkml C 1 1 3+ 

pdumC 43+pkkml C21 3 ; 

pi C23  -pdumC03+pkkml C23+pdumC23*pkkml C 123+ 
pdumC43+pkkml C223 ; 

pi C33  -pdumC03*pkkml C33+pdumC23*pkkml C 133+ 
pdumC43+pkkml C233 ; 

pi C43  =pdumC03*pkkml C43+pdumC23+pkkml C 143  + 
pdumC43+pkkml C243 ; 
pi  C53=*pi  C  1  3  ; 

pi C&3  =pdumC53*pkkml C 1 3+pkkml C63  + 

pdumC73*pkkml C 1 1 j+pdumC93*pkkml C21 3 
pi C73  -pdumC 5 3+pkkml C2 3+pkkml C73+ 

pdumC73*pkkml C 123+pdumC93*pkkml C223 
pi C83  =pdumC5 3+pkkml C33+pkkmlC83+ 

pdumC73*pkkml C 133+pdumC93*pkkml C233 
pi C93  spdumC53+pkkml C 43+pkkml C 9 3+ 

pdumC 7 3+pkkml C 143+pdumC93+pkkml C243 
pi C 103-pi C23 ; 
pi  C  1 1  3-pi C73 ; 

pi C 123=pdumC 103+pkkml C23+pdumC 12 3+pkkml C 123+ 
pdumC 143+pkkml C 223 ; 

pi C 133-pdumC 10 3+pkkml C33+pdumC 123*pkkmlC133+ 
pdumC 143+pkkml C233 ; 

pi  C 143-pdumC 10 3+pkkml C43+pdumC 123+pkkml C  143  + 
pdumC 143+pkkml C243 ; 
pi C 153-pi C33 ; 

pi C 163-pi C83;  V’, 

pi C 173-piC 133; 


piC183-pdumC153#pkkmlC33+pdufliC173*pkkmlC133+ 
pkkffll C 183+pdumC 193*pkkml C233 | 
piC193«pdumC153*pkkmlC43+pdumC173*pkkmlC143+ 
pkkflil C 193+pdumC 193*pkkml C243 ; 
pi C203»pi C43 ; 
piC213-piC93; 
piC223-piC143; 
pi C233spi C 193 ; 

pi  C243«pdumC203*pkkml  C43+pdumC223*pkkml  C 143+ 
pdumC243*pkkml C243 ; 

/*  CALCULATE  FIRST  MEASUREMENT  PREDICTION  */ 

zhat»velm*denom; 

zdiff Ci  3*zicCi  3-zhat; 

/*  COMPUTE  THE  SATE  FOR  ERRONEOUS  TIME  */ 

/*  MEASUREMENTS  */ 

pkl“(piC03  <  0)  ?  -piC03  :  piC03; 

pk3* (pi C 123  <  0)  ?  -pi C 123  i  piC123; 

pk5»(piC243  <  0)  ?  -piC243  :  piC243; 

pll=(pkl  >  pk3)  ?  pkl  :  pk3; 

p  •  (pll  >  pk5)  ?  pll  t  pk5; 

pgate«sqrt <p) *velm; */ 

x=p; 

if  <x  !-  0.0) 

C 

if  <x  <  1.0)  x-1.0; 
do 

< 

oldx=x ; 

x*(  x  +  p/x  )*0.5; 
temp=oldx-x; 

rabs=(temp  <  0)  ?  -temp  :  temp; 

> 

while  (rabs  >  0.0); 

> 

pgate""x*velm; 

gate*3. 0* (pgate+rr Ci 3 ) ; 

printf  (" \nMATRIX  PDUM  s  \n">; 
printmat (pdum,5,5) ; 
printf ("\nMATRIX  PI  :  \n"); 
printmat (pi ,5,5) ; 
printf ("\nZDIFF  is  :  ”); 
printf  ("\7.e  \n"  ,zdif  f  Ci  3 ) ; 
printf ( "\nGATE  is  :  " ); 
printf  ("\54e  \n",gate);  */ 


/*  EDIT  INVALID  TIME  MEASUREMENTS  #/ 
zgate* (zdif f Ci  3  <  0)  ?  -zdiffCi3  t  zdiffCi  3; 

if  (zgata  >■  gats) 

{ 

for  <ii«0;ii<nmax; ii++>  giCii3-0.0; 
zdiffCi 3-999.0; 

/*  TAB  FOR  INVALID  TIME  MEASUREMENT  */ 

y 

/*  CALCULATE  ESTIMATE  BASED  ON  ONE  */ 

/*  MEASUREMENT  PREDICTION  */ 

for  (ii-0;ii<nmax;ii++> 

xiCii3-xkkfl)lCii3+giCii3*zdiffCi3; 

printf  <"\nVECT0R  XI  i  \n">; 
printvec <xi ,5) ;  */ 

for  <i i-O; i i<nmax ; i i++) 

xkkinlCii3-xi£ii3; 
pkkmlCii 3 -pi Cii 3; 

> 

for  < j j-nmax ; j jCmaxsq; j j++)  pkkml C j j 3-pi C j j 1 ; 
if  (i  --  3)  goto  156; 

y  /*  end  JS  loop  */ 

/*  NOTE  i  CALLED  ORISINAL  X (0/-1 ) , XKKM1 .  */ 

/*  UPDATED  AFTER  1  MEASUREMENT  CALLED  IT  XI,*/ 

/*  THEN  MADE  XKKM1  AND  WENT  THRU  ITERATION  */ 

/*  AGAIN.  AFTER  YOU  HAVE  UPDATED  XKKM1  FOR  */ 

/*  EACH  MEASUREMENT  XKK-XI  AND  PKK-PI.  */ 

156:  for  <i i-O; i iCnmax ; i i++) 

C 

xkkCii 3-xi Cii 3; 
pkkCii 3-piCii 3; 

> 

for  ( j J-nmax; j J<maxsq; j j++) 

£ 

pkkC j j3=pi C  j  j3; 

> 

printf ("\nUPDAT I NG  VECTORS  INTERVAL  ...  \nH>; 

printf  C'NnVECTOR  XKKM1  :  \n">; 

printvec (xkkml ,5) ; 

printf <"\nMATRIX  PKKM1  :  \n"); 

printmat  <pkkml ,5,5) ; 

printf ("\nVECT0R  XKK  :  \n">; 

printvec (xkk ,5)  ; 

printf ("\nMATRIX  PKK  :  \nH>; 


printmat <pkk,5,5) j 


/*  RECALCULATE  TIME  MEASUREMENTS,  and  FORM  */ 

/*  ABSOLUTE  VALUE  OF  RESIDUALS.  «/ 

■for  (i=0;  i<4$  i++) 

/*  EDIT  INVALID  TIMES  FOR  ADAPTIVE  MANEUVER  */ 

/*  ROUTINE  */ 

< 

if  (zdiffZil  <  999.0) 

C 

xaxkkmlC03-xbCi 3} 
y*xkkmlC23-ybCi 3 ; 
zaxkkmlC43-zbCi3; 

/*  d#nomasqrt <x#x+y*y+z*z) ;  */ 

denom»x*x+y*y+z*z ; 

x®denom; 

if  (x  !»  0.0) 

€ 

if  <x  <  1.0)  x»1.0; 
do 

f 

oldx=x ; 

x*(  x  +  denom/x  )*O.Sj 
tomp«oldx-x; 

rabs=*<temp  <  0)  ?  -tamp  :  temp; 

> 

while  (rabs  >*  l.a-12); 

> 

denom=x ; 

z  h  a  t  =*  ve  1  m*d  en  om ; 
temp=*zicCi 3-zhat; 

zdi-f  f  Ci  3=  (temp  <  0)  ?  -temp  :  temp; 

3 

else 

zdi-f  f  Ci  3-0.0; 
nzdi  f  -f-=l ; 

> 

> 

/*  THIS  FUNCTION  COMPUTES  THE  ADAPTIVE  Q  MATRIX  */ 
i-f  (i  curve  »  1) 

< 

bb=xkkmlC33*xkkmlC33; 
dd=xkkml C13*xkkmlC13; 
al=bb+dd; 


3 


SR 


.n  :vl‘Av.  . 


bd*xkkml£13*xkkml£33; 

•l«dd*sigacc/al+bb*sigcc; 

•12"bd*sigacc/al-bd#sigcc; 

•2"bb*si gacc/a 1 +dd*si gcc } 

q£03»g2*el; 

q£13*g3*el ; 

q£23-g2*al2; 

q£33-g3#«12; 

q£63>a2*el; 

q£73«q£33; 

q£83-a2**12; 

qC 123*g2*e2; 

q£133*g3#«2; 

q£lB3-a2*«2; 

> 

•Isa 

< 

bb=*gamma£03*gammaC03 ; 
dd*gammaC 1 3*gamma£ 13; 
q£03*bb#covw£03 ; 
q£ 1 3sgamma£03*covw£03*gamma£ 13; 
q£23»0.0;  q£33«0.0; 
q£63=dd*covwE03 ; 
q£73»0.0;  q£S3-0.0; 
q£ 123*bb*covw£ 1 3 ; 
qC 133=gammaC03*covwC 1 3*gamma£ 1 3 ; 
q£ 183«dd*covw£ 1 3 ; 

> 

q£53»q£13;  q£103«q£23;  q£U3-q£73; 

qC153=qC33;  qE163»q£B3;  q£173=q£133; 

/*  printf  < "NnMATRIX  Q  s  \n"); 

printmat (q,5,5)  ;  */ 

/*  IF  ALL  TIME  MEASUREMENTS  EXCEED  GATE,  BYPASS  •*/ 
/*  ADAPTIVE  MANEUVER  ROUTINE.  */ 

if  (nzdiff  !«  O.O) 

{ 

zdifav»(zdiff E03+zdif f E13+zdiff E23+zdiff E33)/nzdiff ; 

/*  printf  C'NnZDIFAV  is  *  ">; 

printf <"\Xe  \n",zdifav>;  */ 

/*  DO  NOT  PERFORM  ADAPTIVE  MANEUVER  IF  */ 

/*  FILTER  HAS  NOT  ACHIEVED  THE  STEADY  STATE.  * 
if  (kk  >  3) 

< 


/*  IF  ZDIFAV  MEETS  CRITERIA  TRANSFER  OUT  /* 
/*  OF  ADAPTIVE  MANEUVER  ROUTINE.  */ 
if  (zdifav  >  value  icont  <  5) 


/*  INCREASE  GAIN  */ 
for  ( j*0; j<maxsq; j++) 

pkkml C j3»pkkC j3+qC j  3; 
printf <"\n INCREASE  GAIN  AND  PERFORM"); 
printf ( “  ADAPTIVE  ...  \n">; 
printf <"\nMATR IX  PKKM1  :  \n"); 
printmat (pkkml ,5,5) ;  */ 

/*  PERFORM  ADAPTIVE  MANEUVER  BY  */ 

/*  REITERATING  SAME  TIME 
SLOT. 

icont+»l ; 
goto  adapt; 

> 

> 

> 

icont«0; 
nzdif f =4; 

xdiff C03»xkkC03-truedC03; 
xdiff Cl 3=xkkE23-truedC13; 
xdiff C23=xkkC43-truedC23; 

temp-xdiff C13*xdiff C 1 3/pkkml C 123 ; 
if  (temp  <  limvar)  icurve«0; 
else  i cur ve^i;  */ 

f printf (fpe, "\n  Xd  Xe  Xe  Xe" , 

kk,xdiffC03,xdiffC13,xdiffC23) ; 

/*  WRITE  */ 

printf <"\n VECTOR  ERROR  :  XDIFF  YD IFF  ZDIFFXn") 
printvec (xdif f , 3) ;  */ 

/*  PREDICTIONS  FOR  PKKM1  */ 

for  (i*0; i<nmax ; i++) 

C 

phipkkCi 3=pkkCi 3+pkkCi+53*phisC03; 
phipkkCi+53=pkkCi+53; 

phipkkCi+103=pkkCi+103+pkkCi+153*phisC23 ; 

phipkkCi+153=pkkCi+153; 

phi pk k C i +20 3 *p kk C i +20 3 ; 

> 

for  <i»0; iCnmax ; i++) 

{ 

in“i*nmax ; 

pktempCin3“phipkkCin3+phipkkCin+l 3*phisC03; 


pktemp C i n+1 3 »phi pkk C i n+1 1 ; 

pktemp£in+23»phipkkCin+23+phipkkCin+33*phisC23 ; 
pktemp Cin+33*phipkkCin+33 ; 
pktemp Cin+43«phipkkCin+43; 

> 

■for  <  j*0;  j<maxsq;  j++)  pkkml  C  j  3=pktempC  j  3+qC  j  3  \ 

printf  ("\nPREDICTIONS  FOR  PKKM1  ...  \n">; 
printf  < "NnMATRIX  PKKM1  x  \n">; 
printmat (pkkml ,5,5) ;  */ 

/*  CALCULATE  NEW  XKKMl  */ 

■for  (ii*0; i i<nmax ; i i++> 

{ 

xkkml Ci i 3sxkk£ii3; 

/*  pl»»qrt <f abs (pkkCii 3 > > ;  */ 

> 

x  kkml C03=xkkml C03+phi sC03*x kk  C 1 3 ; 
xkkml C23»xkkmlC23+phisC23*xkkC33; 

print-f  ( "\nNEW  XKKM1  ...  (for  use  in  next  time”); 
printf  ("  interval )  \n") ; 
printf ("\nVECTOR  XKKM1  s  \n”>; 
printvec (xkkml ,5) ;  */ 

f  printf  (f  pp ,  "\n  Xd  */.e  */.e  7.e"  , 

kk,pkkC03,pkkE123 , pkk £243 )  ; 

f printf (fpxyz , "\n  Xd  Xe  Xe  Xe" , 

kk,xkk[03,xkkE23,xkkC43)  ; 


>  /■*  end  TIME  loop  */ 

f cl ose (fpxyz ) ; 
f close (f pe) ; 
fclose(fpp) ; 
f cl ose (f pz i c ) ; 
f close (fptrj ) ; 

printf < "press  a  key  and  ENTER  to  continue 
scanf ( "Xs" ,  &c) ; 

printf ( "\nquit  ?  CY/N3  x"); 
scanf ("Xs" ,  &c) ; 

if  (c=»*'n'  !!  c^ss'N’)  goto  menu; 


>  /*  end  MAIN  PROGRAM  */ 


pnntvac  (vector  ,dim) 
int  dim; 

daub la  vactorCl; 

< 

int  i,  j; 

■for  ( j*0; j<dim; j++)  print-fC'Xa  %ls"  , vectorC 
print-f  ("\n") ; 

> 

printmat (matrix , row, cal ) 
int  row, col; 
double  matrixCl; 

C 

int  i , j ; 

•for  (i*0; i<row; i++> 

<. 

■for  <  j=0;  j<col ;  j++> 

%ls" , matrix  Ci*col+JD  f "  "); 
print-f  ("Xn") ; 


tra  jec  (narr ay ,  hydro  ,  xb , yb , 2b , x  kkml ,  jstart ,  j  and ,  wd ,  g ,  worg  > 
double  hydroC 3,xbC3,ybC3,zbC3,xkkmlC3,wd,g; 
int  narr ay, worg, jstart, jend; 

< 

int  pli,i, start , end ; 

double  ppl ,rl , sqrt () ,zcs,zicC43,ziC43; 
double  xd,yd,zd,v,den,velm,gg; 

static  double  xnC63={-3. 01 ,  —2.0,  -1.5,  1.0,  -0.6, 

0.03; 

static  double  ynC63=£0.0,  0.0228,  0.0668,  0.1357, 

0.2743,  0.53; 

static  double  snC53={43. 8596,  11.3636,  7.25689, 

2.891352,  2.658873; 

double  dataC53,wr; 
int  runn,i8; 

FILE  *fptrj,  *f  open  O  ; 

FILE  *fpzic,  *fopen(>; 
int  j,jj; 

/*  THIS  ROUTINE  COMPUTES  TRUE  TRAJECTORY  OF  TORPEDO 
dataC03=true  x  position,  dataC13=  true  y  position 
dataC23=true  z  position,  w»turn  rate 
dataC33=x_veloci ty ,  dataC43=y_veloci ty 
jstart=time  slot  where  maneuver  starts 

jend=time  slot  where  maneuver  ends  */ 

dataC03=xkkml C03;  dataC13=xkkmlC23;  dataC23=xkkml C43 
dataC33=xkkml C 1 3 ;  dataC43=xkkml C33 ;  velm=l . 0/vel ; 
r 1=0. 49; 

wr=twopi*wd/360. 0;  gg=32.2*g;  start= jstart ;  end=jend 

printf("\n  %s  \n", "start  execution  o-f  trj.c"); 
f ptr j=f open  ( "tr  j .  dat "  ,  "w"  >  ; 
f pzic=f open ( "zic. dat " , "w" >  ; 


•for  ( j j=0; j j< 150; j j++) 

< 

/*  GET  HYDROPHONE  ARRAY  COORDINATES  */ 
if  (narray  !=  0) 


if  <dataC03  >  swO) 

i8=0; 

el  se 

if 

<dataC03 

> 

swl )  i 8=12; 

else 

if 

(dataZOJ 

> 

sw2)  i 8=24; 

el  se 

if 

<dataC03 

> 

sw3)  i8=36; 

el  se 

if 

(dataC03 

> 

sw4)  i8=48; 

else  iS=60; 

for  ( i =0 ; i <  4 ; i ++ ) 

j=3*i  +i  8; 

xb  C  i 3=hydroC  j  3 ; 


AD-A167  871 


UNCLASSIFIED 


KALMAN  FILTER  REAL  TIME  TRACKING  USING  MICROPROCESSORS 
AND  A  NEN  LANGUAGE  FOR  MILITARY  SOFTMARE(U)  NAVAL 
POSTGRADUATE  SCHOOL  NONTEREV  CA  J  A  FERREIRA  27  MAR  86 


ybCi 3»hydro£ j+13; 
zbEi 3“hydroE j+23; 

> 

> 

/*  COMPUTE  THE  TRUE  TIMES  AND  THE  TRUE  POSITIONS 
WRITE  TRUE  TIME  and  TRUE  POSITIONS  */ 
printf  <"%«"  ,".")  | 
fprintf  <f ptr j  ,  "\n  */.»  %•  %•", 

data£03 , dataC 1 3 , data E 23 ) ; 

for  <i«0;  i<4;  i++) 

{ 

xd-data£03-xb£i 3 ; 
yd*dataE 1 3-ybEi 3 ; 
zd»data£23-zbEi3; 
dan«sqrt <xd*xd+yd*yd+zd*zd> ; 
zi  Ci 3*v*l m*den; 

printf ("\nTRUE  TIMES  :  \n“)| 
printvac (zi ,4) | 

printf  C'XnTRUE  POSITIONS  s  \n"); 
printvec (data, 3) ;  */ 

/*  GET  NOISE, SCALE  AND  ADD  TO  TRUE  */ 

/*  MEASUREMENT  TIME  */ 

ppl»317. 0*rl i 

pli-ppl; 

rl*ppl-pli ; 

v*r  1  ; 

j»0; 

if  <v  >  0.3)  v»l . 0-r  1 ; 
r2s  if  (v  <  ynEj+13)  goto  rS; 
j+-l; 
goto  r2; 

r 8:  v=*(v-ynE  j3)*sn£  j3+xn£  j3; 
if  (rl  >-  0.5)  v-(-v) | 

zcs*  <v/3. 00) *0.00001; 

printf ("\nRANDOM  NUMBER  s"); 
printf  ("*/.e  \n"  ,zcs);*/ 
z i c  £ i 3 =zcs+2 i £ i 3 ; 

} 

f  printf  (fpzic,  "\n  '/.e  V.e  V.e  7.e"  , 

zicC03,zic£13,zicE23,zic£33) ; 

/*  NEW  DATA  */ 

1 ine ( j j ,0, 150, data) ; 


f close (f ptr j > | 
f close(-fpzic)  | 

> 

1  i  ne  <  j , start , and , dat a  > 
doubls  dataClj 
int  j, start, and | 
t 

x  -f  <  <  j  >■  start)  &&  <j  <  end)) 
< 

dataC03+»l . 31*dataC33 ; 
dataC 1 3+-1 . 31*dataC43  ; 


APPENDIX  G 


LISTING  OF  PLOT . C  -  PLOTTING  ROUTINE 

/ ********************************************************** / 
/*  */ 

/*  PLOTTING  ROUTINE  -  FILE  :  PLOT.C  (IBM-PC)  */ 

/*  */ 

/ ********************************************************** / 

#include  "stdio.h" 

(♦include  "math.h" 

(♦include  "limits. h" 

(♦include  "kalman.h" 
mai n ( ) 
i 

f 1 oat  i x  Cnp 1 2 , var Cnp 1 3 , sum , sumsq ; 

int  i ,col ,row,inc,c,tt, choice, narray,runn, op, cc; 

float  xCnptl,  yCnptl,  z Cnptl ,tCnpt J ,xx ,yy ,zz ; 

float  xmax ,xmin,ymax , ymin , power , yaux ; 

f 1 oat  hei ght , wi dth , x  seal e , yscal e; 

float  ymaxt ,ymint ,xmint , xmaxt; 

double  xkkmlC53; 

FILE  *fpt,  *fopen<); 

FILE  *fpxyz,  *fopen<); 

FILE  *fpe,  *fopen<); 

FILE  *fpp,  *fopen<>; 

FILE  *fpin,  *fapen<); 

f pin=f open ( "tr j. inp" , "r") i 

fscanf  (fpin,  "Xd  Xd"  ,&narray,8erunn)  ; 

f scanf (f pin , "XI f  XI f  XI f  XI f  XI f" , 

&xkkmlC01fScxkkmlC21y8(XkkmlC41,8(xkkml[;i], 

Scxkkml  C33) ; 
f close (fpin) ; 

CLR  SCRN; 

printf ( "\n  INITIAL  CONDITIONS  FOR  RUN  (♦  " ) ; 

printf ( "Xd" ,runn) ; 
puts ( "\n  \n" ) ; 
if  (narray*“0) 

printf <"\n  CASE  is  SINGLE  ARRAY."); 

else  printf <"\n  CASE  is  MULTIPLE  ARRAY."); 

printf("\n  Xs  Xe  Xs  Xe  Xs  Xe  \n","  xpos  =  ", 

xkkmlCOl,"  ypos  =  " ,xkkmlC23 , "  zpos  =  ", 
xkkml C43 ) ; 

printf ("\n  Xs  Xe  Xs  Xe  \n","  xvel  =  ", 

xkkmlCll,"  yvel  *  " , x kkml C33 ) ; 


choice8®! 


printf  <"XnXn\n\nXn\n\n\n\nXn\nPress  any 

key  to"); 

V 

printf <"  continue 

...  "); 

$ 

c«keyrd <) ) 

XL 

m 

for (  ;  ;  ) 

$ 

CLR  SCRN; 

printf  <" 

PLOT  OPTIONS  i  "); 

printf  ( “ \n “ ) | 

ui 

printf  C'Xn 

1) 

X_EST  vs  TIME  SLOT 

printf  <"\n 

2) 

Y  EST  vs  TIME  SLOT 

»); 

^ jj 

printf  <"\n 

3) 

Z  EST  vs  TIME  SLOT 

Bn 

printf  <"\n 

4) 

Y  EST  vs  X  EST  " ) ; 

printf  ("\n 

5) 

X  ERR  vs  TIME  SLOT 

«); 

/• , 

printf  <"\n 

6) 

Y  ERR  vs  TIME  SLOT 

">; 

me 

printf ("\n 

7) 

Z  ERR  vs  TIME  SLOT 

"); 

printf  ("\n 

8) 

X  VAR  vs  TIME  SLOT 

»>; 

A 

printf  <"\n 

9) 

Y  VAR  vs  TIME  SLOT 

»); 

•/« 

printf  <"\n 

10) 

Z  VAR  vs  TIME  SLOT 

"); 

printf ( "\n 

11) 

PKK  X  vs  TIME  SLOT 

(  , 

\  *( 

printf  ("\n 

12) 

PKK  Y  vs  TIME  SLOT 

"); 

printf  <"\n 

13) 

PKK_Z  vs  TIME_SLDT 

"); 

printf ("\n") ; 

% 

printf  ("\n 

14) 

TRUE  TRAJECTORY  " ) 

■ 

f 

printf  <"\n 

13) 

CHANSE  PLOT  Cdot  <- 

>  linear"); 

> 

'wm 

printf <"  (default9 

linear) 3") ; 

printf  C'Xn 

16) 

EXIT  "); 

M* 

printf <“Xn“); 

printf ("Xn 

enter  value  (1-16) 

:  ">; 

.*/ 

mops  CUR_SAVE; 

/.■ 

scanf  ( "%2d 11 ,  &op )  ; 

*.  » 
►  . 

if  ( op< 1  !  1  op >16) 

t 

•V 

% 

BELL; 

$ 

CUR_REST ; 

» 

■  ^ 

goto  mop; 

•  - 

printf  ( "XnXsXdXs" , 

"  option  is 

"  .op, 

> 

"  CY/N3  i 

»); 

scanf ( " %a " , &cc  > ; 

jv 

if  <cc ! ■ *y '  M 

cc  ! 

a'Y’) 

‘•x 

BELL; 

v\ 

CURJREST; 

& 

goto  mop; 

fee 

> 

9 

printf  ("\n 

wai t  " ) ; 

v' 
*  . 

switch (op) 

t 

s 

i  * 

V 

case  1: 

f pxyz«f open ( "xkk. dat" , "r"> ; 

f  scanf  (fpxyz  ,  "\n  Xd  Xf  Xf  Xf  "  ,&tt,&xx  ,&yy,&zz) ; 
xC03*xx;  tC03*tt; 

ymax^xx;  ymin*xx;  xmax»npt;  xmin*0.0; 

■for  (i»l; i<npt; i++) 

{ 

fscanf  (fpxyz  ,  "\n  Xd  X-f  X-f  Xf", 

Sett  ,8«xx  ,&yy,&zz>  ; 
xCil«xx;  tCil*tt; 

if  (xx>ymax)  ymax*xx; 

•Isa  if<xx<ymin)  ymin®xx; 

printf  ("Xs",".  ")  j 

y 

plot (ymax , xmax ,xmin,ymin,x ,t , choice) ; 
biosset (3,30) ; 

printf  <"  Xs  " ,  "X_EST  vs  TIME_SL0T") ; 

biosset (4,8) ; 

printf (“Xs"," (FT)") ; 

fclose(fpxyz) ; 

break; 

> 

case  2i 

<. 

fpxyz-fopen ("xkk. dat" , "r"> ; 

fscanf  (fpxyz,"\n  Xd  Xf  Xf  Xf  "  ,&tt,8«xx  ,S<yy,8<zz>  ; 
xC01»yy;  tCOl-tt; 

ymax^yy;  ymin»yy;  xmax=*npt;  xmin=0.0; 
for  (i*l ;  i<npt;  i ■+•■♦■> 

C 

fscanf (fpxyz , "\n  Xd  Xf  Xf  Xf", 

Sett ,  S<xx  ,  &yy  ,!tzz  >  ; 
xCi3»yy;  tCil»tt; 

if  (yy>ymax)  ymax ay y; 
else  if(yy<ymin)  ymin=yy; 

printf ("Xs",". "> ; 

> 

plot (ymax , xmax ,xmin,ymin,x , t , choice) ; 
biosset (3,30) ; 

printf ("  Xs  ","Y_EST  vs  TIME_SLQT" )  ; 

biosset (4,8) ; 

printf ("Xs" , " (FT) "> ; 

f close (fpxyz ) ; 

break; 

> 

case  3s 
C 


Ira 


m  P 

■  •  _  *  •  •  •  . 


*  •  ■ » ’ » *  •  •  •  * » *  "m  ■  .*•  - **  »  ■  •»■*«* •**  **-  »'*  »*»  »*»  ,*»  „*»  .*•  .*»  a 


f pxyz«f opan ( "xkk. dat "  ,  "r">  j 

facanf  (fpxyz  ,  "\n  Xd  Xf  Xf  Xf ’’ ,&tt  ,&xx  ,&yy  ,&zz  >  f 
xC03-zz»  tC03-tt; 

ymax«zz|  ymin-zzj  xmax-nptj  xmin«0.0; 
for  <i«l| i<nptj i++> 

facanf (fpxyz , "\n  Xd  Xf  Xf  Xf”, 

&tt , &xx , &yy , &zz ) | 
xCi3«zzj  tCi3»tt; 

if  (zz>ymax)  ymax**zz; 
visa  if<zz<ymin)  ymin“zz; 

pr intf <”Xs”,“. ”> * 

> 

plot  (ymax ,xmax ,xmin,ymin,x , t , choice) ; 
biosset (3,30) ; 

pr  intf  < "  Xs  ”  ,”Z_EST  vs  TIME_SL0T">; 

biosaat (4,8) ; 

printf ("X«" , " (FT) ")  j 

fclosa (fpxyz ) ; 

braakf 

caaa  4s 

i 

fpxyz-f opan ( "xkk. dat " , "r"> ; 

facanf  (fpxyz  ,  “\n  Xd  Xf  Xf  Xf  "  ,&tt,&xx  ,Scyy,&zz) ; 
tC03»xxj  xC03*yyf 

ymax*yyj  ymin*yyj  xmax«xxj  xmin=xx; 
far  (i*l| i<npt; i++) 

facanf <fpxyz,"\n  Xd  Xf  Xf  Xf" , 

&tt ,&xx ,&yy ,&zz ) ; 
tCi3*xx;  x  Ci 3*yy j 

if  (yy>ymax)  ymax»yy; 
alsa  if(yy<ymin)  ymin=*yy; 

if  (xx>xmax)  xmax«xx; 
alsa  if  (xx<xmin)  xmin^xx; 


printf ("Xs" ") ; 

> 

plot (ymax , xmax ,xmin,ymin,x , t , choice) ; 


biosaat (3,30) $ 

printf ("  Xs  ","Y_EST  vs  X_EST"); 

biosaat (4,8) ; 

printf ("Xs"," (FT)") 5 

biossat (22,63) ; 

printf ("Xs"," (FT) ") ; 


break; 

> 

case  Si 

l 

fpe-fopen("xdiff  .dat'Vr") ; 

f  scant  (f  pe,  "\n  %d  Xf  Xf  Xf  "  ,  Sett  ,fcxx  ,8<yy  ,&zz ) 
xC03«xx;  tC01*tt; 

ymax«xx;  ymin*xx;  xmax>npt;  xmin™0.0; 
for  (i*l; i<npt; i++) 

< 

f  scant  (f  pe,  "\n  Xd  Xf  Xf  Xf  " , 

8ctt ,&xx  , &yy ,&zz)  ; 
xCil*xx;  tCil*tt; 

if  (xx>ymax)  ymax»xx; 
else  if(xx<ymin)  ymin«xx; 

pr intf  ("Xs", ; 

y 

plot (ymax ,xmax , xmin , ymin ,x , t ,  choice) ; 
biosset (3,30) ; 

printf  ( “  Xs  " , "X_ERR  vs  TIME_SL0T" ) ; 

biosset (4,8) ; 

printf ("Xs"," (FT) ") ; 

fclose(fpe); 

break; 

> 

case  bi 

<. 

f pe*f open ( "xdif f . dat " , "r " ) ; 

f  scant  (f  pe,  "\n  Xd  Xf  Xf  Xf  "  ,&tt  ,8.xx  (&yy,&zz> 
xC03=»yy;  tC01»tt; 

ymax*yy;  ymin»yy;  xmax»npt;  xmin=0.0; 
for  (i«l; i<npt; i++> 
i 

f scant (fpe,"\n  Xd  Xf  Xf  Xf" , 

Sett ,  &xx  ,&yy  ,8 <zz )  ; 
xCil»yy;  tCil*tt; 

if  (yy>ymax)  ymax«yy; 
else  if (yy<ymin)  ymin»yy; 

printf ( "Xs" ,"."); 

y 

plot (ymax , xmax , xmin , ymin , x , t , choice) ; 
biosset (3,30) ; 

printf  ("  Xs  ","Y_ERR  vs  TIME_SL0T" ) ; 
biosset (4,8) ; 
printf ("Xs", " (FT) ") ; 
f cl ose (f pe) ; 
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break} 

> 

case  7* 

< 

f  pe«f  open  <  "xdif f .  dat "  ,  "r  " ) ; 

fscanf  (fpe, "\n  Xd  Xf  Xf  Xf  “  ,&tt  ,&xx  ,8<yy  ,8«zz ) ; 
xC03»zz|  tC03«tt; 

ymax«zz|  yminmzzf  xmsx~npt;  xmin*O.Oj 
■for  <i*l|  i<npt|  i++) 

-f  scan-f  (fpe,  "\n  Xd  Xf  Xf  Xf"  , 

&tt  ,&xx  ,  &yy  ,  &zz )  ; 
xCi3-zz|  tCi 3“tt } 

if  (zz>ymax)  ymax*zz; 
else  if(zz<ymin)  ymin»zz} 

printf  ( "Xs" , ") } 

> 

plot (ymax , xmax , xmin , ymin ,x , t , choice) ; 
bi osset (3,30) ; 

printf <"  Xs  ","Z_ERR  vs  TIME_SL0TK); 

bi asset (4,8) ; 

printf  ("Xs'V  (FT) '') ; 

fclase(fpe) ; 

break} 

> 

case  81 

i 

fpe-f open < "xdif f .dat","r")  ; 

fscanf  (fpe,"\n  Xd  Xf  Xf  Xf  "  ,fctt  ,&xx  ,8cyy  ,&zz ) ; 

x  C03»xx ;  tC03»tt; 

var  C03=*0. 0}  sum«0.0;  sumsq«0. 0; 

ymax*xx;  ymin*xx}  xmax>npt}  xmin^O.O; 

for  (i*l} i<npt; i++) 

fscanf (fpe,"\n  Xd  Xf  Xf  Xf", 

&tt ,  &xx  ,  &yy  , &zz  > ; 
xCi3=*xx;  tCil»tt} 

sum+*»xCi];  sumsq+*x  C  i  3*x  C i  1  ; 

var  Ci ]»( (tCiD+1.0) #sumsq-sum*sum) /( (tCi3+1.0)*tCil) 
xx=var Ci 3 ; 

if  (xx>ymax)  ymax=xx; 
else  if(xx<ymin)  ymin«xx; 


printf ("Xs",". ")  ; 

> 


plot (ymax , xmax , xmin ,ymin , var , t , choice) 


biosset (3,30) : 
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printf  <"  Xs  ","X_VAR  vs  TIMEJBLQT" ) ; 

bioaaat (4,7) ; 

printf ("Xs"  ,  "  (FTaq)  ") ; 

f close(f pa) ; 

break; 

J 

casa  9: 

< 

fpe“fopen  ("xdiff .  dat",  "r")  ; 

fscan-f  (f pa,  "\n  Xd  Xf  Xf  Xf  "  , &tt  ,&xx  ,8cyy ,&zz ) ; 
xC03«yy;  tE03»tt; 

var C03»0.0;  sum=*0.0;  sumsq^O.O; 

ymax»yy;  ymin»yy;  xmax=npt;  xmin^O.O; 
for  (i«l; i<npt; i++) 

C 

facanf  <fpa,''\n  Xd  Xf  Xf  Xf" , 

&tt ,  &xx ,  Styy  ,&zz ) ; 
xCi3*yy;  tCi3=tt; 

sum+“xCi3;  sumsq+“x Ci 3*x Ci 3 ; 

var  Ci  3s=(  (tCi  3+1 .  0)  #sumsq-sum*sum)  /  (  (tCi3+1.0)*tCi3) 
yy =var C i 3 ; 

if  <yy>ymax)  ymax^yy; 
else  if(yy<ymin)  ymin-yy; 

pr i ntf ( "Xs" 

> 

plot (ymax , xmax ,  xmin ,ymin , var , t , choice) ; 
biosset (3,30) ; 

printf ( "  Xs  ","Y_VAR  vs  TIME_SLOT" )  ; 

biosset (4,7)  ; 

printf ("Xs"," (FTsq) "> ; 

f close (fpe) ; 

break; 

> 

case  10: 

C 

f  pes'f  open  ( "xdi  f  f .  dat"  ,  "r  " )  ; 

f  scanf  <f  pe,  "\n  Xd  Xf  Xf  Xf  "  ,8<tt  ,&xx  ,&yy,&zz)  ; 

xC03»zz;  t C03=tt ; 

varC03=0.0;  sum=0.0;  sumsqs'O.O; 

ymax=zz;  ymin=zz;  xmax=npt;  xmin=0.0; 

for  (i=l; i<npt; i++> 

< 

f scanf (fpe, "\n  Xd  Xf  Xf  Xf", 

&t t , &x  x , &yy ,  8<z  z  > ; 
xCi3=zz;  tCi3=tt; 

sum+*xCi3;  sumsq+=x Ci 3*x Ci 3 ; 
var Ci 3  =  ( (tCi3+1.0) *sumsq-sum*sum) /( (tCi3+1.0)*tCi3) 


zz*varCi J> 

if  (zz>ymax)  ymax*zz; 

•Isa  if(zz<ymin)  ymin^zz; 

printf  ("Xs"  ,"."); 

> 

plot  (ymax  ,xoiax  , xmin ,  ymin ,  var ,  t , choica)  $ 
biosset (3,30) ; 

printf <"  Xs  " , "Z_VAR  vs  TIME_SLQT" > ; 

biosset (4,7) | 

printf ("Xs“,"  (FTsq)  ") ; 

f closa(fpa) ; 

break; 

> 

casa  1 1 i 

< 

fpp«fopan("pkk.dat","r"> } 

f  scanf  (f  pp,  "\n  Xd  Xf  Xf  Xf " ,&tt ,&xx ,&yy ,&zz ) 
xCOl-xx;  tC03«tt; 

ymaxaxx;  ymin*xx;  xmax*npt;  xmin«0.0; 
for  (i*l; i<npt; i++) 
f 

f scanf (fpp, "\n  Xd  Xf  Xf  Xf", 

S«tt  ,&xx  , &yy ,&zz) ; 
xCi3»xx;  tCil*tt; 

if  (xx>ymax)  ymax»xx; 
slsa  if(xx<ymin)  ymin-xx; 

printf ("Xs",". "> ; 

> 

plot (ymax ,xmax , xmin , ymin ,x , t , choice)  ; 
biosset (3,30) ; 

printf ("  Xs  ","PKK_X  vs  TIME_SL0T" )  ; 

biosset (4,7) ; 

printf  (•'Xs","  (FTsq)  '*) ; 

f close(f pp) ; 

break; 

> 

case  12: 

C 

f pp“f open ( "pkk. dat" , "r" ) ; 

f scanf (fpp, "\n  Xd  Xf  Xf  Xf " ,&tt ,&xx ,&yy ,&zz ) 
xCOl-yy;  tC03=tt; 

ymax^yy;  ymin=yy;  xmax-npt;  xmin=0.0; 
for  (i=l; i<npt; i++) 
i 

f scanf (fpp, "\n  Xd  Xf  Xf  Xf" 

Z>± t ,  &x  x  ,  &yy , &z  z  ) ; 
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x  Ci J*yyj  tCi3«tt; 

if  (yy>ymax)  ymax*yy; 

•Isa  i*(yy<ymin>  ymin»yy; 

print*  <  ">Cs"  ,"."); 

> 

plot (ymax , xmax , xmin ,  ymin ,x , t ,  choice) ; 
bi osset (3,30) ; 

print* (”  %s  " ,"PKK_Y  vs  TIME__SLOT") ; 

biossat (4,7) ; 

print* ("Xs","  (FTsq)  ") ; 

*closa (*pp) ; 
braak; 

> 

casa  13i 
t 

*pp“*open ("pkk.dat" , "r") ; 

*scan*  (*pp ,  "\n  7.d  X*  7.*  7.*"  ,&tt  ,&xx  ,Seyy,S(Zz) 
x[03*zz;  tC03*tt; 

ymax>zz;  ymin»zz;  xmax=npt;  xmin=0.0; 

•for  ( i  =  1 5  i<npt;  i++) 

C 

*scan*  (*pp,  “\n  7.d  7.*  7.*  7.*", 

Sett  ,S<xx  ,  &yy  ,S<zz  )  ; 
xCi3«zz;  tEiD-ttj 

i*  (zz>ymax)  ymax-zz; 
alse  i* <zz<ymin>  ymin^zz; 

print* ( "7s" , ") ; 

} 

plot (ymax , xmax , xmin ,ymin ,x , t , choice)  ; 
biosset (3,30) ; 

print*  <"  7.s  ","PKK_Z  vs  T I  ME__SL0T  "  )  ; 
biosset (4,7) ; 
print*  ("7.s","  (FTsq) 

* close (*pp) ; 
break; 

> 

case  14: 

i 

*pt=*open ("trj.dat", "r") ; 

*scan*  (*pt ,  " \n  7.*  7.*  X*  "  ,8exx  ,  8<yy ,  Sezz )  ; 
xC03=xx;  yC03=yy; 

ymax=»yy;  yminayy;  xmax=xx;  xmin=xx; 
for  (i=l: i<npt; i++) 
i 

fscan*  (*pt ,  "\n  7.*  7.*  7.*  "  ,Sexx  ,8eyy  ,8«zz  )  ; 
xCi3=xx;  yCiD=yy; 


if  (yy>ymax)  ymax«yy; 

■Is*  if(yy<ymin)  ymin»yy; 


if  (xx>xmax)  xmax*xx; 
els*  if  (xx<xmin)  xmin»*xx; 

pr i ntf  ("Xs",". "); 

} 

plot (ymax ,  xmax ,xmin ,ymin , y ,x , choice)  ; 
biosset (3,30) ; 

printf  < "  */. s  ","TRUE_Y  vs  TRUE_X"); 

biosset (4,8) ; 

printf  ("7.s" ,"  (FT)  ")  ; 

biosset  (22,63)  ; 

printf  ("7.s" ,"  (FT)  ")  j 

f close (fpt) ; 

break; 

> 

case  15: 

£ 

printf ("\n  CHANGE  PLOT  (  enter  option  )") 

printf ("\n") ; 

printf ("Xn  1)  LINEAR  INTERPOLATION  BETWEEN") 

printf ("  POINTS"); 

printf ( "\n  2)  SIMPLE  DOT" ) ; 

printf ( "\n") ; 

printf ("\n  ENTER  choice  :  "); 

scanf ( "7d" ,&choice) ; 
goto  skip; 
break; 

> 

case  16:  goto  ending; 
default: 

£ 

goto  skip; 
break; 

> 

>  /*  end  case  •*/ 

CUR_M V ( 4 , 67 )  ; 

printf  ("7.s  7.d","#  ",runn); 

CUR_MV (5,67)  ; 

if  (narray=*=0)  printf  <  "SINGLE" )  ; 
else  printf ("MULTIPLE") ; 

c=keyrd ( ) ; 
biosini (3)  ; 
skip: 

}  /*  end  for — forever — loop  */ 


/*  Function  to  draw  two  axis. 

/* 

/*  ***  NOTE  *** 

/*  The  upper  left  corner  of  the  screen  is  0,0  and  the 
/*  bottom  right  corner  is  199,639. 

/* 


int  axis (ul row, ul col , lrrow,lrcol ,mantx ,manty) 
int  ulrow,ulcol ,lrrow,ircol ; 
double  mantx C 3 ,mantyC 3 ; 

£ 

int  count, i row ,i ,iaux , icol , tcount , cine ,rinc ,hinc; 
double 

del tax ,del tay ,ymax ,xmin ,x ,y ,rowmax , rowmin , col max , col min; 
double  del tar ,deltac,aux; 

ymaxsmanty £03 ; 
xmin*mantx£13; 

del tay»0. 1* (manty [03-mantyC 1 3 ) ; 
deltax=0. 1* (mantx EOl-mantx C 1 3 ) ; 

rowmax*lrrow;  rowmin=ulrow;  del tar»0. 1* (rowmax -rowmin) ; 
colmax*lrcol ;  colmin«ulcol ;  del tac»0. 1* (col max-col min) ; 
r i nc=del tar+0. 5;  cincadel tac+0. 3; 


/*  do  vertical  -  line  with  scale  */ 

i=ulcol ; 

for  (irow  =  ulrow+1;  irow  <=*  lrrow;  irow++>  bioswd (irow, i) ; 
for  (irow  ■  ulrow;  irow  <■  lrrow;  irow+*rinc) 

for  (tcount=i-4;  tcount  <=  i;  tcount++) 

b i oswd (irow, tcount ) ; 

/*  do  horizontal  -  line  with  scale  */ 

i*lrrow; 

for  (icol  *  ulcol+1;  icol  <=»  lrcol;  icol++)  bioswd  (i  ,  icol ) ; 
for  (icol  *  ulcol;  icol  <=  lrcol;  icol+=cinc) 

for  (tcount=i;  tcount  <=  i+4;  tcount++) 

bi oswd ( tcount , i col ) ; 


/*  write  vertical  scale  */ 
y=ymax ; 

for  (irow  =  ulrow;  irow  <=  lrrow;  irow+=rinc) 
£ 

i =irow/8. 0+0. 5; 
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bi osset <i ,0) ; 
printf  <  "X.  If " ,y)  j 
y-«deltay; 

> 

/*  writ*  horizontal  scale  *7 
x«xmin; 

-for  (icol  »  ulcol;  icol  <  =  lrcol;  icol+»cinc) 
{ 

i»icol*0. 125-1.5; 
bi osset (24, i ) ; 
print-f  ( MX.  If  ",x>  j 
x+«del tax: 


/#  done  */ 
return (0) ; 

> 

plot (yymax ,xxmax ,xxmin , yymin ,x , t , choice) 
float  yymax ,xxmax , x C 1 , t C 3 , xxmin, yymin; 
int  choice; 

float  xscale, yscale, height, width, bx , by; 
float  ymax , xmax ,ymin,xmin; 

double  xaux ,yaux ,mantx C23 , mantyC23 , loglO ( > ; 
double  aux,pow(> ,auxl; 

int  i ,inc,row,col , ixexp , iyexp ,c ,ol drow,newrow, i i , iimax 
int  col  1 ,colO,rowl , rowO, irow,oldcol , icol ,newcol ; 
float  deltac,deltar,delta,ang; 

/*  DETERMINE  THE  SCALING  */ 

ymax^yymax;  x max *x xmax;  ymin=yymin;  xmin^xxmin; 
if  (ymaxaayniin) 

ymax  =»ymax +5.0; 
ymin=ymin-5. 0; 

> 

if  (xmax«xmin) 


xmax=xmax+5. 0; 
xmin*xmin-5. 0; 

> 

hei ght =ymax -ymi n ; 
wi  dth*»xmax  -x  mi  n ; 
x  seal ea560. 0/wi dth ; 
yscal e»- 160. 0/height ; 
by^lSA. 0-ymin*yscale+0. 5; 
bx=600. 0-x max *x sc ale+0. 5s 


/*  DETERMINE  EXPONENT  and  MANTISSA  */ 
xaux*1.0; 

aux^abs (xmax ) ;  aux l«abs (xmin) ; 
if  (aux  <  auxl)  aux«auxlj 
if  (aux! >0.0) 

C 

xaux*loglO (aux )  ; 
ixexpaxaux; 

if  (ixexp<0)  ixexp-»lj 

aux«ixexpj 

xaux^powC 10.0, aux) ; 

> 

man t  x  C  0 1  «x  max  /  x  aux ; 
mantx  C 1 3«xmin/xaux ; 

yaux»1.0; 

aux-ab* (ymax ) ;  aux laabs (ymin) ; 
if  (aux  <  auxl)  aux*auxl) 
if  (aux! *0.0) 

< 

yaux*loglO (aux) j 
iy ex p*y aux | 

if  (iyexp<0)  iyexp-*lj 

aux*iyexp| 

yaux-pow (10.0, aux ) ; 

> 

man t y C  0  3  «y  max / y aux ; 
manty C 1 3*ymi n/yaux ; 

printf ("\nX*" , "Press  any  key  to  see  plot."); 
c»keyrd ( ) ; 
biosini  (6) ; 

ax is (24, 40, 184, 600, mantx , manty) ; 

y aux ay aux ; 
biosset (3,6) ; 
printf ("%. le" , yaux) ; 


x  aux  ax  aux  ; 
biosset (22,68) ; 
printf ("%. le" ,xaux) ; 


/a#*##**##*#***####*###*#***##****#****#***#*#****##***#**#/ 
/*  */ 

/*  PLOT  USING  LINEAR  INTERPOLATION  BETWEEN  POINTS  */ 
/*  */ 

/*********#*#*******#**#****##****#***#********#***#**-»***#/ 

if  <choic»“»l) 

col 0«tC03*x seal e+bx ; 
r owO«x C 0 3 *y sc al a+b y ; 
bioswd (rowO,colO) ; 

for  (i*l ; i<npt; i++> 
i 

col l»t Ci 3*xscale+bx ; 
rowl*x  Ci 3*yscal e+by; 
dal tac»col 1-colO; 
del  t  ar  *=r  ow  1  -r  owO ; 

if  Cabs (del tac) >=abs <del tar > ) 

{ 

ang-del tar /del tac; 
i  imax=«abs  (cal  1-calO)  ; 
icol=l; 
col »cal 0; 
if  (call<calO) 
f 

ical*-l; 

> 

for  (ii=»0;ii<iimax;ii++) 

< 

col+=icol ; 

del  ta^col -col 0; 

row=ang*del ta+rowO+O. 5; 

/*  if  (choice=*=l> 

if  (abs(deltar)  >  5  &&  abs(deltac)  >5) 
goto  discont; 

>  */ 

bioswd (row, col > ; 

> 

>  /*  end_if  abs (del tac) >=abs  (del  tar)  */ 

else  if  (abs (del tac) <abs (del tar ) ) 
i 

ang=del tac /del  tar ; 
i i max sabs (rowl-rowO) ; 
irow=l ; 


row*rowO; 
if  (rowl<rowO) 

{ 

irow«-l; 

> 

-For  (i i*0; i i<i imax ;  i i++) 

£ 

row+«irow; 

del ta*row-rowO; 

col  aang#del  ta+col  0+0. 5; 

/*  if  (choice»l) 

£ 

if  (abs(daltar)  >  5  &&  abs(deltac)  >5) 
goto  discont; 

>  */ 

bi oswd (row , col ) ; 

> 

y  /*  end_else_if  */ 
col O*col 1 ;  r owO«r owl ; 

>  /*  end_if  for  i *0,200  */ 
y  /*  end_if  choice* 1  a/ 

/ a********************************************************* / 


/*  a/ 
/*  This  portion  is  to  be  used  if  you  want  just  the  */ 
/*  paints  without  linear  interpolation.  */ 
/*  a/ 


/a*************************###**#******************#**#*#**/ 


if  (choice»2) 

£ 

for  (i*0; i<npt; i++) 

£ 

col*tCi 3*xscal e+bx ; 
row*x£i 3*yscale+by; 
bioswd (row, col ) ; 

> 

y 

goto  termino; 
discont: 

£ 

biosini (3) ; 

printf ( "\n  DISCONTINUOUS  DATA  FOUND, 
printf  ( "  CHANGE  OPTION  to  SIMPLE"); 
printf ("-DOT  PLOT  and  TRY  AGAIN."); 
printf  ("  Press  any  key  to  continue  . 
c*keyrd ( ) ; 

> 

termino: 

> 


"); 
")  . 
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APPENDIX  H 


LISTING  OF  THE  BATCH  FILES  -  IBM-PC 
/#*#**#***********#*#*#*****#*****-»****#*#-»*****##*#####«*•»/ 


/*  */ 

/*  Batch  file  "CE. BAT"  (IBM-PC).  Contains  the  */ 

/*  sequence  o-f  calls  to  compile  the-file  KR.C  .  */ 

/*  */ 

/*  USAGE:  "ce  kr"  */ 

/*  */ 

/*#*********###*#**##****#-»*#***#**#**-»«#-»*#*****#*-»-»*##-»**/ 


1  c  1  XI 
lc2  XI 

link  cs+Xl+biosio+tr+dostime,Xl ,nul ,lcms+lcs 

erase  Xl.obj 

pause 


/e*********************************************************/ 
/*  */ 
/*  Batch  fils  "KAL.BAT"  (IBM-PC).  Contains  ths  */ 
/*  sequence  of  calls  to  execute  the  -files  KR.C  */ 
/*  (Kalman  Filter  program)  and  PLOT.C  (the  */ 
/*  plotting  routine).  */ 
/*  USAGE:  "kal "  */ 
/*  */ 


graphics 
:  loop 
k r  “4000 

rem  DO  YOU  WANT  TO  PLOT  RESULTS  ? 
pause 

plot  “8000 

rem  DO  YOU  WANT  TO  TRY  ANOTHER  CASE  ? 

rem  CTL-BREAK  will  stop  execution.  Press  any  other  key  to 

continue. 

pause 

CLS 

goto  loop 


APPENDIX  I 


SIMTERM  -  PROGRAM  DOCUMENTATION 

This  Appendix  contains  the  listing  of  the  file  "readme" 
found  in  the  PC-SIG  LIBRARY  (Public  Domain  Library) ,  disk 
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1.  Introduction 

Fssl  free  to  make  copies  of  this  program.  There  is 
no  restriction  on  the  distribution  as  long  as  the  copyright 
notices  are  left  on  the  source  and  that  there  is  no  charge 
for  the  distribution.  This  program  is  made  available  to 
users  of  IBM-PCs  (and  compatibles  since  I  really  like  the 
COMPAQ/PLUS).  No  guarantees  are  made  or  implied.  If  you  have 
any  questions  or  suggestions,  please  give  me  a  call  and  I 
will  try  to  answer  them.  A  recent  acquisition  of  an 
interrupt  driven  communication  driver  from  the  University  of 
British  Columbia  Computer  Center  has  enabled  me  to  remove 
the  requirement  that  you  have  the  IBM  Async  Communications 
Support  package  which  included  a  driver  I  used  to  use.  This 
version  of  SIMTERM  is  therefore  self-contained  and  requires 
no  other  software  to  run.  Have  fun  using  it  and  **HAPPY 
COMPUTING**. 

This  floppy  contains  all  the  software  for  the 
terminal  simulator  for  the  IBM  PC.  The  main  function  of 
this  program  is  to  provide  a  reasonable  terminal  simulation 
on  the  IBM  PC  of  an  HP-like  terminal  to  a  UNIX(TM)  system. 
The  file  'simterm.exe'  is  the  load  module  and  when  invoked, 
assumes  a  default  of  1200  baud,  1  stop  bit,  no  parity,  and 
flow  control  (X0N/X0FF) .  The  only  programs  that  have  to  be 
on  the  floppy  are  SIMTERM.EXE,  HELP. TXT ,  and  MENUS.  If  you 
have  a  VENTEL  or  a  Hayes  modem,  the  file  BOARDS  can  be 
copied  over  as  a  starter  for  a  list  of  BBSs.  The  BOARDS  file 
is  set  up  for  a  VENTEL  autodialer.  The  dialing  strings  will 
have  to  be  changed  to  conform  for  the  Hayes  standard. 

If  you  have  DOS  2.0,  put  SIMTERM.EXE,  HELP. TXT, 
BOARDS  and  MENUS  in  a  subdirectory  called  \simterm.  That 
way,  if  you  change  directories  during  a  SIMTERM  session,  by 
specifying  a  path  for  any  type  of  file  transfer,  SIMTERM 
will  be  able  to  find  its  way  back  to  these  two  important 
files.  If  you  don't  have  DOS  2.0,  then  just  IGNORE  the 
occasional  "directory  not  found"  messages,  they're  harmless. 

2.  Command  Line  Parameters 

Optional  parameters  on  the  command  line  will 
override  the  defaults  as  follows] 
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'-a'  put  ths  simulator  into  ths  ADM3a  emulation  mode 

■for  those  systems  which  don't  support  TERMCAP. 

*-b'  Causes  SIMTERM  to  assume  you  are  using  a 

monochrome  display,  even  though  you  may  not  really 
have  one. 

‘-B'  will  cause  the  transmit  and  receive  bu-f-fers  to  be 

cleared  when  a  BREAK  (End  key)  is  sent.  This  is 
mainly  -for  high  baud  rate  lines  where  there  is 
still  junk  in  the  bu-f-fers  that  you  don't  want  to 


*-cn'  specifies  which  COMx  device  you  want  to  use.  The 

de-fault  is  1  (use  CCMi). 

*-d'  Enter  direct  connect  mode.  SIMTERM  won't  wait  -for 

carrier  detect.  Use  this  option  i-f  you're 
connected  directly  to  the  -emote,  with  no  modem. 
You  can  also  use  this  option  when  SIMTERM  seems  to 
hang  at  startup,  with  the  "Estanlish 
communications  link"  message.  This  may  be  case 
with  Hayes  modems  i-f  you  don't  have  them  optioned 
to  raise  the  carrier  detect. 

'-D'  When  this  -flag  is  used,  SIMTERM  will  ignore  DEL 

(127  Ascii)  characters,  and  the  character 
immediately  following  the  DEL.  We  use  it  because 
we  have  a  noisy  line. 

specifies  that  the  EPSON  printer  does  not  have  the 
GRAFTRAX  option.  Therefore  when  using  'ibmpr'  to 
route  stuff  to  the  printer,  the  code  will  simulate 
underlining,  subscripts,  and  superscripts.  Default 
is  an  EPSON  with  GRAFTRAX. 

‘  -g '  Causes  SIMTERM  to  assume  you  are  using  a  graphics 

display,  even  though  you  may  not  really  have  one. 

* -h '  SIMTERM  will  operate  in  the  ’ hal f-dupl ex '  mode; 

i.e.,  it  will  echo  the  characters  the  user  types 
since  the  host  system  does  not. 

’-in'  specifies  the  IRQ  (interrupt  request  level)  to  be 
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used.  De-fault  is  4,  but  some  boards  (like  the 
□uadram)  use  IRQ3  -for  COM2,  so  in  this  case 
SIMTERM  would  have  to  be  invoked  with  the 
parameters  '-c2  -i3'  to  set  it  up  correctly. 

’-m'  Tells  SIMTERM  that  you  have  a  Hayes  modem  instead 

of  the  default  VenTei  modem.  This  will  enable 
SIMTERM  to  use  the  directory  dialing  features 
(Alt-V) 

’-M'  Redefines  the  cursor  control  keys  compatible  with 

a  set  of  UNIX-like  utilities  on  a  PRIME  computer 
at  UofVa. 

’-nxxxx'  if  you  have  a  Ventel  or  Hayes  modem,  this  option 
will  automatically  dial  the  number  when  the 
simulator  is  started. 

'-o'  turns  off  the  error  messages  caused  by 

communications  line;  e.g.,  framing  error  and 
invalid  parity. 

*-px'  specifies  the  parity  where  ‘n'-none,  ‘o'-odd, 

'e'-even,  'm'-mark,  and  's '-space. 

'-PFfil'  Use  file  "file"  as  a  script  source  file  (see 
SCRIPTS  below) 

'-PDscr'  Execute  script  "scr"  upon  start-up  (see  SCRIPTS 
below) 

* -r '  this  causes  SIMTERM  to  wait  for  the  horizontal 

retrace  when  writing  to  the  screen.  If  you  notice 
'sparkles'  on  your  graphics  monitor,  invoke 
SIMTERM  with  this  option  and  it  should  clear  up. 

* -snnnn '  specifies  'nnnn'  as  the  baud  rate  (e.g.,  -s300) . 

’ -Sn '  specifies  the  number  of  stop  bits  (1  or  2) 

’-vxxxx'  specifies  the  file  (xxxx)  to  be  used  when  the 
Alt-V  options  is  invoked  to  dial  BBS.  The  full 
path  name  of  the  file  should  be  specified. 
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'-wn'  sat  tha  word  langth  to  'n'.  De-fault  is  8  (7  data  + 

1  parity). 

*-xn*  this  option  will  turn  o-f-f  the  XQN/XOFF  -flow 

control.  Tha  program  will  send  an  XOFF  when  the 
receive  bu-f far  is  almost  -full  and  then  an  XON 
whan  it  has  emptied  out.  It  will  also  recognize  an 
XOFF  -from  the  host  to  stop  transmitting  and  then 
an  XON  to  start  again. 

'-kfilename'  will  read  in  the  -file  '-filename'  to 
initialize  the  function  keys.  The  file  is  of  the 
form 

n*»string  of  chars 

where  0<=n<=9 

ex ample 
0=who^V 
l=who ! wcAV 

If  the  string  is  terminated  with  a  ,AV'  (chr(174) 
—  obtained  by  holding  down  the  ’Alt'  key  and  typing  174  on 
the  key  pad),  a  'carriage  return '  will  be  appended. 

For  example: 

If  you  want  to  run  at  9600  baud,  even  parity, 
without  waiting  for  carrier  detect 
simterm  -s9600  -pe  -d 

If  you  want  to  use  COM2, (and  it  requires  IRQ3)  at 
300  baud  and  run  script  "xyzzy"  from  file  "scr2" 

simterm  -s300  -c2  -i3  -PFscr2  -PDxyzzy 

If  you  are  using  a  Hayes  modem  and  want  a  number 
dialed  when  SIMTERM  is  started,  use  the  following  command 
1  ine: 

simterm  -m  -d  -n 12145551212 
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3.  Program  Functions  (Unix  specifics) 


Once  loaded,  the  program  will  use  all  available 
memory  above  the  module  for  save  space  for  paging  through 
previous  output.  I  have  256K  on  my  system  and  this  provides 
about  20  pages  (~500  lines)  of  saved  text.  The  TERMCAP  entry 
for  the  terminal  is  in  the  file  ' termcap. ibm * .  This  should 
either  be  added  to  your  termcap  library  or  the  'shell' 
variable  TERMCAP  should  be  set  to  it  before  trying  to  use 
any  package  that  requires  cursor  addressing.  The  TERMCAP 
entry  makes  use  of  the  fact  that  * vi '  (and  'curses')  will 
use  a  special  sequence  to  enter  and  leave  the  control  mode. 
The  simulator  uses  these  sequences  to  redefine  the  cursor 
control  keys  so  that  they  can  be  used  in  that  mode;  e.g.,  in 
* vi ’  the  "up  arrow"  will  send  in  a  "k",  while  the  "PgDn" 
will  send  in  a  M>SD"  to  scroll  the  display  down,  etc.  For 
those  of  you  who  are  running  on  UNIX  systems  that  use 
"terminfo",  there  is  a  file  "terminf o. ibm"  which  has  the 
appropriate  definitions. 

Output  to  the  printer  can  be  initiated  under 
program  control  with  a  filter  similar  to  *hp'  which  is  used 
for  HP  terminals.  The  source  file  for  'ibmpr'  (which  is  the 
filter  that  use  on  UNIX)  is  on  the  floppy.  You  should 
compile  this  UNIX  and  then  use  for  controlling  output  to  the 
printer.  A  typical  use  of  this  filter  is: 

nroff  -T37  file  I  ibmpr  -p 

The  ' -p '  option  says  to  route  the  output  to  the 
EPSON  printer.  The  * -i '  option  will  cause  underlined  text 
to  come  out  in  italics.  The  ' -c '  option  is  used  to  invoke 
the  compressed  mode  (132  characters/1 ine) . 

If  those  escape  sequences  are  output,  they  will  be 
displayed  appropri atel y  on  the  CRT  and  output  to  the  printer 
correctl y. 


4.  User  Defined  Function  Keys 

There  are  10  user  definable  functions  keys  that 
will  send  a  character  sequence  of  up  to  30  characters.  To 
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display  the  currant  contents  of  the  keys,  hold  the  'Alt'  key 
down  and  type  '  =  To  change  the  contents,  hold  the  'Alt' 
key  and  type  '-'  and  follow  directions.  To  invoke  the 
functions  keys,  hold  down  the  'Alt'  key  and  type  the 
corresponding  digit  (e.g.,  ‘Alt  3').  A  user  can  define  a 
file  with  the  contents  of  the  keys  and  cause  it  to  be  read 
in  with  the  k'  option  on  the  command  line. 

5.  Up load /Down load  (Unix  Specific) 

This  is  used  to  transfer  'text'  files  (ones  that 
only  contain  printable  ASCII  and  tabs)  between  the  PC  and 
the  host.  To  remember  which  way  the  data  flow,  just  remember 
that  the  IBM  PC  looks  'up'  to  UNIX,  while  UNIX  looks  'down' 
on  the  PC.  To  move  files  between  the  PC  and  UNIX,  hit  'Cntl- 
PgUp *  to  transfer  PC->UNIX.  Answer  the  prompts.  Hit  'Ctrl- 
PgDn *  to  move  from  UNIX-  >PC.  As  the  transfer  is  taking 
place,  a  message  will  indicate  the  #  of  bytes  transferred. 
When  going  UNIX->PC,  there  will  be  more  bytes  on  the  PC  disk 
than  on  UNIX  because  of  added  *  end-of-1  ine '  and  'end^-of- 
text'  bytes.  This  can  only  be  used  to  transfer  TEXT  files. 

6.  Upload /Down load  (Straight  ASCII  &  XMODEM) 

The  program  can  be  used  to  connect  to  BBSs  and 
any  other  systems  that  support  async  devices.  To  download  a 
file  to  the  PC,  go  through  the  normal  dialog  on  the  BBS  and 
then  when  it  is  waiting  for  you  to  hit  return  to  start  the 
transfer,  press  Alt-C  to  open  the  'copy'  file  that  will 
receive  the  data.  You  will  be  put  back  to  the  original 
screen  where  you  can  hit  return.  At  the  end  of  the  data 
transfer,  hit  Alt-C  to  close  the  copy  file. 

To  upload  a  file  from  the  PC  to  a  BBS,  again  go 
through  the  dialog  to  setup  the  transfer  and  when  the  BBS 
is  ready,  press  Alt-D  to  specify  the  data  file.  This  will 
then  'dump'  the  file  to  the  comm  line.  At  the  end  of  the 
transfer,  you  may  have  to  type  in  a  control  sequence  to 
indicate  that  you  are  done.  This  option  will  request  a 
'delay'  between  characters  so  that  you  do  not  overrun  the 
receiving  system.  Use  '0'  for  no  delay;  a  value  of  *1'  is 
probably  sufficient  for  most  systems  that  can  not  handle  the 
normal  baud  rate.  Experiment. 
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SIMTERM  supports  ths  XMODEM  protocol  for  data 
transfer  from  a  BBS.  To  invok#  this  option,  hit  Alt-R  to 
receive  with  XMODEM  at  the  point  that  the  sender  is  ready. 
You  will  be  prompted  for  the  file  to  receive  into  and  then 
the  PC  will  initiate  the  transfer  of  data.  Transmitting  with 
XMODEM  is  invoked  with  Alt-T  and  prompts  will  follow. 

7.  Script  files 

Script  files  are  used  to  allow  SIMTERM  to  do 
mindless  interactive  login  and  other  such  sequences.  A 
Scriptfile  can  be  created  by  your  favorite  word  processor 
and  SIMTERM  will  compile  it  for  you,  and  save  the  compiled 
version  so  you  don't  have  to  re-compile  at  each  run.  Script 
files  read  much  like  English,  so  they  are  both  easy  to 
understand  and  create. 

Script  Commands  Available 

Each  of  the  following  commands  can  appear  in  a 
script  file.  Note  that  each  separate  command  must  be 
separated  by  a  space,  tab  or  newline.  The  compiler  ignores 
extra  white-space,  so  you  can  have  indenting,  multiple 
commands  per  line,  or  whatever  you  want.  Syntax  notes:  "str" 
in  the  fallowing  denotes  a  string  constant  delimited  by 
quotes.  A  string  can  have  multiple  words.  To  get  a  quote 
character  into  a  constant,  write  it  as  \".  To  get  a 
backspace,  write  it  as  \\.  Although  one-word  strings  need 
not  have  quotes,  they  should,  so  that  they  are  not  confused 
with  keywords  (commands).  Non-quoted  strings  generate 
warnings  from  the  compiler. 

name  "str"  -  defines  an  entry  point  for  the 

script  that  you  can  use  to  identify  this  part,  i.e,  if  a 
"name  whuxlb"  appears  in  the  file,  then  you  can  start 
running  the  script  by  using  the  name  "whuxlb".  (See  "running 
scripts"  below)  If  you  choose  "whuxlb"  from  the  pop-up  menu 
later,  the  script  will  start  executing  at  the  statement 
immediately  following  the  "name"  statement. 

send  "str"  -  sends  the  string  to  the  remote 

computer,  with  a  carriage  return  tacked  onto  the  end.  Some 
escapes  can  be  placed  into  the  string,  as  follows:  an  "\1" 
causes  a  one  second  pause  when  it  is  encountered.  "\m"  sends 


Copyright  @  1982,1984 


Jim  &  Eric  Holtman 


SIMTERM  Instructions 


Psgsi  8 


a  carriags  rsturn.  "\j"  sands  a  line-feed.  "\c"  supprassas 
the  carriaga  raturn  that  is  normally  tackad  onto  tha  and  of 
a  string.  Remember,  though,  you  need  to  type  TWO  backspaces 
to  get  one,  so  these  sequences  should  appaar  as  \\l,\\m,  etc 
in  your  -file. 

dial  "str"  -  dials  the  string  on  a  VenTel 

dialer.  Obviously,  if  you  don't  have  a  ventel ,  don't  use 
this  command  (Fake  it  with  "send").  For  example,  to  dial 
"3395"  on  a  Hayes,  put  a  line  like  "send  ATDT3395"  in  your 
file. 


say  "str"  — -  outputs  "str"  onto  the  screen,  so 

that  you  can  put  tracer  statements  in  the  script. 

goto  "str"  -  branches  to  str.  labels  to  be 

branched  to  must  be  followed  by  a  colon,  l.e.  early  in  the 
script  you  have  a  line  "label:"  all  by  itself.  Later,  you 
can  call  "goto  label"  (no  colon)  to  branch  back  up  to  the 
earlier  statement 

gosub  "str"  -  like  goto,  but  when  a  "return" 

statement  is  encountered  later,  the  program  branches  BACK  to 
the  statement  that  follows  the  gosub.  Gosubs  may  be  nested. 

return  -  returns  from  a  "gosub"  statement 
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input  "str"  -  Prompts  the  user  with  "str", 

reads  one  line,  and  sends  it  to  the  remote  computer. 

settime  "str"  -  sets  the  timeout  value  to  "str" 

seconds.  Default  is  fifteen. 

quit  -  execution  is  returned  to  SIMTERM. 

There  is  an  understood  "quit"  at  the  end  of  file 


The  Logical  Operators  (IF/CASE) 

Besides  the  COMMANDS,  the  script  compiler 
recognizes  two  logical  operators,  the  if /else  statement  and 
the  case. 


IF  SYNTAX 
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if  "str"  stmnt  1  also  stmnt  OR  if  "str"  t 

stmnt 

stmnt 

OR 

a 

if  "str"  stmnt 1  (no  ELSE)  >  also  stmnt 

OR  }  el  se  •£ 
stmnt 

a 

> 

Whan  ancountarad,  the  program  will  scan  the  output 
from  the  remote  computer  until  either  it  finds  “str"  or  it 
times  out.  If  it  finds  "str",  stmntl  is  executed,  if  it 
times  out,  stmnt2  is  executed.  Execution  then  drops  down  to 
the  next  statement,  unless  the  stmntl  or  stmnt2  contained  a 
goto  or  gosub  or  quit. 

stmntl  or  stmnt 2  may  be  either  single  commands 
from  above,  or  multiple  commands  delimited  by  £  and  >  (note 
white  space  around  the  brackets.  If  the  else  following  the 
if  is  missing,  then  execution  falls  to  the  next  statement. 
Note  that  ifs  may  be  nested.  For  example 

if  "«tr "  dial  "4244"  else  if  "str2"  dial  "4324" 

Be  aware,  however,  that  input  is  only  scanned 
once,  so  if  "str2"  appears  before  "str"  in  the  preceeding 
example,  both  tests  will  fail!! 

CASE  SYNTAX 

The  case  statement  is  like  multiple  simultaneous  IFs. 
It  allows  you  to  set  up  a  few  strings  to  be  searched  for 
SIMULTANEOUSLY,  with  an  action  specified  to  be  executed  if 
and  when  one  of  the  strings  is  found.  Syntax  is 

case 

strl:  action 
str2:  action 
■ 

otherwise  action 
caseend 
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Action  can  bs  sithsr  a  singls  statement  (say, 
send,  dial,  or  even  an  if,  or  another  case.)  or  multiple 
statements  surrounded  by  <  and  >  (like  the  if  clause.).  For 
example,  if  you  have 

case 

eric:  dial  "eric's  #" 
holtman:  dial  "another  #" 
caseend 

and  the  input  is  "djsfhj  gfjg  eric  holtman"  then  "eric's  #" 
will  be  dialed  and  the  case  will  quit.  Note  that  "holtman" 
is  not  matched  since  a  case  statement  executes  as  soon  as 
any  of  the  strings  is  found. 

Script  Examples 

Call  number  3395,  wait  for  "login"  prompt.  If  I 
get  it,  gosub  to  a  login  procedure.  If  I  don't  then  say 
something,  then  quit.-  The  login  subroutine  will  send  my  id, 
then  prompt  for  my  password,  send  it,  then  return. 

name  example 
dial  3395 

if  "login"  gosub  "subr"  else  < 
say  "3395  didn't  answer" 
quit 

> 

quit 
subr : 

send  "id3434" 

input  "Your  password,  sire?  " 
return 

Note  that  after  the  "return"  the  next  statement  to 
be  executed  will  be  "quit". 

Compiling  Scripts 

To  use  the  example  above,  copy  it  out  to  a  file 
called,  for  example,  "prog".  Then  invoke  SIMTERM  with  a  - 
PFprog  flag,  like  so:  "simterm  -PFprog" .  This  tells  SIMTERM 
that  you  wish  to  use  scripts  from  the  file  "prog".  If  no  -PF 
flag  appears,  SIMTERM  will  read  the  file  "scripts"  which  is 
where  you  may  want  to  keep  compiled  scripts.  After  SIMTERM 
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gets  running,  type  an  Alt-L.  This  informs  SIMTERM  that  you 
wish  to  uss  a  script.  It  will  sxamins  ths  fils  "prog"  and 
datsrmin#  that  it  nesds  to  bs  compilsd.  Tail  SIMTERM  what 
fils  you  want  to  put  ths  compilsd  stuff  in,  thsn  sit  back 
and  watch  for  srror  msssagss.  After  it  compiles,  hit  return 
and  proceed  to  the  next  step.  Remember  the  file  name  you 
used  for  the  compiled  scripts,  and  use  it  next  time  you  run 
SIMTERM,  so  that  you  don't  have  to  recompile  everything 
again. 

Using  a  script 

After  you  type  Alt-L,  or  after  the  compile  is 
finished,  a  menu  of  all  "name"  strings  will  pop  up.  If 
you're  still  using  the  example,  only  one  name  ("example") 
will  be  there.  Using  the  up  and  down  arrows,  make  your 
choice,  then  hit  the  space  bar.  SIMTERM  will  then  execute 
your  script  starting  at  that  named  point. 

To  execute  a  script  from  startup,  use  the  -PD 
flag.  For  example,  to  run  script  "example"  from  the  file 
"prog.scr" ,  type 

simterm  -PFprog.scr  -PDex ample 

8.  Functions  Available  from  the  keyboard 

All  these  functions  can  be  accessed  from  the 
keyboard  by  typing  the  appropriate  character.  If  you  forget 
the  commands,  Alt-H  will  bring  up  a  menu  for  you.  Just 
follow  the  menu  selection  process  until  you  find  the  desired 
function.  Note  that  the  menu  will  tell  you  which  key  to  use 
to  activate  that  function  in  the  future.  I.e. ,  the  menu  has 
Alt-L  next  to  the  script  choice  to  remind  you  that  you  can 
use  Alt-L  instead  of  the  menus  to  activate  the  script 
functions. 

FI  Will  'toggle'  the  display  so  that  usually 
non-di splayed  control  characters  are  displayed  in  reverse 
video  using  the  upper  case  letter  that  they  are  equivalent 
to. 


F2 

positioned  at. 


Will  ’print'  the  line  that  the  cursor  is 
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F3 

Send  a  BREAK 

F8 

Freezes 

the 

Handy 

to  save 

some  data  on 

the 

it  as 

you  do 

other 

commands. 

display  above  the  cursor, 
upper  portion  so  you  can  see 


F9  Turns  an  the  row/col umn  display  at  the 
bottom  of  the  screen  so  that  you  know  where  the  cursor  is 
currently  positioned. 

F10  Toggles  XON/XDFF.  A  convenient  way  of 
stopping/restarting  the  output  to  tha  CRT  when  connected  to 
a  system  that  honors  the  XON/XOFF  protocol  for  flow  control. 

Alt-Fl  Will  terminate  SIMTERM,  but  leave  the  DTR 
line  high  so  OR  that  you  can  restart  SIMTERM  and  pickup 
where  you  left  Alt-253  off. 

Alt-F2  Terminate  SIMTERM,  and  drop  DTR  so  that 
the  modem  will  hang  up. 

Alt-F3  Toggles  the  DTR  (data  terminal  ready) 
lead  on  the  communications  line.  This  is  used  to  drop  the 
carrier  at  the  modem  so  that  you  can  radial. 

Alt-F4  Toggles  the  graphics-mode.  This  will 
display  characters  with  their  parity  bits  so  that  the 
'graphics'  characters  of  the  PC  0127)  are  should.  This  is 
used  on  some  of  the  BBSs. 

Alt-F5  Clears  line  25  if  it  becomes  cluttered. 

Alt-F6  Display  various  mode  settings  on  line  25. 

Alt-F7  Resets  the  communications  software.  In 
some  instances  an  interrupt  might  be  missed  and  leave  the 
system  in  a  state  where  it  is  expecting  data  that  will  not 
be  sent.  If  you  suspect  this,  try  an  Alt-F7  before  rebooting 
the  software  to  see  if  it  breaks  you  out  of  the  situation. 

Alt-C  Puts  SIMTERM  in  a  'capture'  mode  so  that 
all  input  received  over  the  communication  line  is  logged  to 
a  disk  file.  Handy  for  picking  up  ASCII  text  from  a 


Copyright  9  1982,1984 


Jim  &  Eric  Holt man 


SIMTERM  Instruction* 


Pagei 13 


system  that  doss  not  support  XMODEM.  It  Mill  prompt  you  -for 
a  -fils  name. 

Alt-D  Will  'dump'  a  ASCII  -fils  on  ths  PC  to  ths 
comm  lins.  This  is  a  way  o-f  'uploading'  to  a  systsm  that 
doss  not  support  XMODEM.  You  can  also  spsci-fy  a  dslay  to 
prsvsnt  ovsrruns. 

Alt-E  Togglss  ths  ’ vi '  mods.  Handy  whan  you 
exit'  SIMTERM  and  thsn  corns  back  into  it  with  UNIX  still  in 
’  vi ' .  Ths  mods  should  bs  TRUE  to  havs  ths  cursor  keys  work 
for  'vi*. 

Alt-F  Entsr  ths  'local'  lins  editing  mode.  This 
allows  you  to  make  corrections  to  a  line  and  then  reenter 
it.  Use  the  cursor  control  keys  to  move  to  the  line,  'Ins' 
and  'Del'  to  fixup  the  lins  and  thsn  hit  return  to  reenter 
ths  lins  and  leave  the  line  edit  mods. 


Alt-H 

function  keys. 

Alt-L 

Alt-R 

Alt-T 


Display  a  'help'  menu  listing  ths  defined 
Invoks  ths  'script'  processor 
Receive  a  fils  using  ths  XMODEM  protocol. 
Send  a  fils  using  ths  XMODEM  protocol. 


Alt-V  Will  display  a  menu  that  is  used  with  the 
'auto-dial'  capability  of  the  Ventel .  Once  in  this  menu, 
select  the  desired  feature. 

Alt-®  Display  ths  user  defined  function  keys 

Alt —  Change  a  user  defined  function  key 

Home  Goto  top  of  file  in  "vi "  (unix  specific) 

Ins  Send  the  current  line  to  remote  (similar 

to  the  ENTER  key  on  an  HP-2621)  OR  if  in  "vi "  toggle  insert 
mods. 

Dsl  Clear  display  from  cursor  to  bottom  OR  if 

in  "vi"  delste  a  character 
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Arrows  liovs  through  scrssn  memory  in  ths 

specified  dirsction 

PgUp  Back  up  on*  pags  through  memory 

PgDn  Go  down  one  page  in  memory 

Cnt r+  Toggle  output  to  printer 

PrtSc 

Ctrl  +  Download  (Unix  >  IBM) 

PgDn 

Ctrl+  Upload  (IBM  - >  Unix) 

PgUp 


9.  Remote  Control  of  SIMTERM 

Certain  sequences  of  characters,  when  received 
over  the  communications  line,  will  cause  SIMTERM  to  execute 
certain  functions,  like  clearing  the  screen,  moving 
characters,  turning  the  printer  on  and  off.  In  the  following 
descriptions  ESC  is  Escape  (ASCII  27),  and  Ctrl-X  denotes 
the  character  generated  by  holding  down  Ctrl  and  typing  X. 
I.e.  Ctrl-A  is  ASCII  1.  Note:  in  the  descriptions  "ESC  H  F" 
for  example  means  ESCAPE,  capital  H,  capital  F,  with  NO 
SPACES. 

ESC  A  —  move  the  cursor  up  one  line 

ESC  B  —  move  the  cursor  down  one  line 

ESC  C  —  move  the  cursor  right  one  space 

ESC  D  —  move  the  cursor  left  one  space 

ESC  E  —  take  SIMTERM  out  of  INSERT  mode 

ESC  F  —  remotely  program  a  function  key.  After 

the  F,  send  a  number  from  1  to  9  to  indicate  which  key  to 
program,  or  a  0  to  program  key  10.  Everything  following  the 
numeral,  up  to,  but  not  including  a  terminating  Ctrl-Z  is 
then  stored  in  that  function  key.  (See  User  Function  Keys, 
above) 

ESC  H  —  moves  cursor  to  top  left  of  screen 
(HOME) 

ESC  K  —  clears  the  rest  of  the  line,  from 

cursor  position  over 
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ESC  J 
ESC  L 

ESC  M 
ESC  P  1 

ESC  P  2 
ESC  P  E 
ESC  P  i 

ESC  P  P 
ESC  P  O 
ESC  a 

ESC  R 

ESC  T  A 

ESC  T  a 
ESC  T  R 

ESC  T  r 

ESC  V  S 

ESC  V  s 
ESC  Cxc 


Instructions 


Pagst IS 


—  cl  oars  display  -from  cursor  to  and  of 
lino,  and  all  linos  bolow  tho  cursor 

—  inserts  a  lino  above  the  cursor  by 
rolling  bottom  linos  down.  Bottom  lino  of 
text  is  lost. 

—  current  lino  is  deleted,  lower  lines 
scrolled  up  to  close  the  gap 

—  turn  on  printer  (Like  Ctrl-PrtSc) . 
Everything  that  now  appears  onscreen 
will  be  copied  to  the  printer 

—  like  P  1,  but  printer  is  put  in 
COMPRESSED  PRINT  MODE. 

—  like  P  1,  but  printer  is  put  in 
EMPHASIZED  PRINT  MODE. 

—  doesn't  turn  printer  on,  just  sets 
ITALICS  mode.  So,  to  print  in  ITALICS, 
first  turn  on  printer  (ESC  ?  1),  then 
have  the  remote  computar  send  this 

—  turn  on  printer,  and  set  into 
PROPORTIONAL  print  mode. 

—  turn  off  printer.  Screen  is  no  longer 
copied  to  printer 

—  Enter  INSERT  mode.  All  subsequent 
characters  will  be  inserted  before  the 
cursor  on  this  line 

—  Deletes  the  character  at  current 
cursor  position. 

Cause  SIMTERM  to  begin  ADM3A 

simulation 

—  Cause  SIMTERM  to  end  ADM3A  simulation 

—  Cause  SIMTERM  to  set  up  to  play  ROGUE, 
so  that  arrow  keys  work. 

—  Cause  SIMTERM  to  end  rogue  playing  set 
up 

—  Cause  SIMTERM  to  set  up  for  vi ,  so 
that  arrows,  Ins,  Del,  PgUp  and  PgDn  do 
reasonable  things  in  this  Unix  text 
edi tor 

—  End  Vi  mode 

—  causes  character  'c'  to  be  printed  x 
number  of  times,  i.e.  to  print  27  B's, 
send  ESC  C  ESC  B,  since  ESC  has  an  ASCII 
value  of  27. 
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ESC  &dB  —  set  screen  attribute  to  a  value  that 
equals  BOLD  on  the  printer  (Actual 
screen  color  varies  from  monochrome  to 
graphics  monitor.) 

ESC  &dD  —  set  attribute  to  correspond  to 
UNDERLINE 

ESC  &dH  —  set  attribute  to  be  SUPERSCRIPT 
ESC  &dL  —  set  attribute  to  be  SUBSCRIPT 
ESC  &d9  —  set  attribute  back  to  normal 

ESC«y  x  —  move  cursor  to  position  y,x:  where  y 
and  x  are  the  ASCII  characters  which 
equal  the  desired  position  plus  32.  I.e. 
to  move  to  position  3,3  send  ESC  *  #  #, 
since  the  ASCII  value  of  #  (35)  minus  32 
i  s  3 

ESC  A  —  request  ID.  SIMTERM  will  send  back  the 
string  "IBM  PC"  terminated  by  a  newline. 


10.  XMODEM  Under  UNIX 

When  using  XMODEM  with  a  UNIX  system,  the  programs 
’xrecv. c'  and  'xtrans.c'  should  be  uploaded  and  compiled  on 
the  host.  These  programs  are  setup  for  the  System  V  release 
of  UNIX  and  may  need  rework  if  used  on  other  systems  (e.g., 
Berkeley).  To  send  a  file  from  the  PC-MJNIX,  typ_  'xrecv 
filename'  on  UNIX  and  when  the  prompt  comes  up  to  start 
transmission,  hit  Alt-T  and  follow  the  instructions. 

To  send  a  file  from  UNIX->PC,  type  'xtrans 
filename'  on  UNIX  and  when- the  prompt  comes  up  to  set  up  the 
receive,  hit  Alt-R  and  follow  the  instructions. 

If  you  are  sending  ASCII  Text  files,  invoke  the 
* xrecv '/' xtrans '  with  the  *-t*  argument  so  that  conversion 
is  done  between  the  DOS  and  UNIX  conventions.  The  XMODEM 
protocol  using  the  *-t'  option  is  the  safest  way  to  transfer 
text  files  between  systems  since  checks  are  made  for 
successful  transmission  and  any  errors  introduced  by  noisy 
lines  will  be  filtered  out. 

Note:  if  the  ’-d'  option  is  given  to  either 
program,  the  following  will  occur.  xrecv  leaves  a 
running  commentary  on  the  connection  status  in  "xr.err", 
and  xtrans  leaves  a  corresponding  file  called  "xt.err".  They 
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are  ummd  for-  debugging  purposes. 

11.  Using  SIMTERM  with  Other  Modems 

SIMTERM  has  been  tailored  to  work  with  the  VENTEL 
or  Hayes  modem.  If  you  are  using  it  with  another  modem  then 
make  sure  that  the  modem  is  optioned  to  present  the  'modem 
ready'  signal.  If  you  want  to  be  able  to  'dial'  the  phone 
number  from  the  keyboard,  you  will  also  have  to  option  it  to 
present  the  'carrier  detect'  signal.  SIMTERM  will  not  get 
past  its  ini tialization  code  unless  it  see  both  the  carrier 
detect  and  the  modem  ready  signals. 

If  you  are  using  an  acoustic  coupler  or  a  212 
'dial  up',  then  when  the  message  'establish  communications' 
appears,  dial  up  the  computer  and  enable  the  'data'  (e.g., 
hit  the  DATA  button  on  the  212  or  put  the  receiver  in  the 
coupler) . 

If  you  are  using  any  modem  (or  are  directly 
connected) ,  and  SIMTERM  seems  to  hang  right  after  the 
"Establish  communications  link"  message,  it  probably  means 
that  your  modem  (or  local  connection)  is  not  outputting  the 
correct  RS-232  signals  for  SIMTERM.  If  the  equipment's  not 
yours,  or  you  don't  understand  how  to  fix  the  problem,  or 
you're  just  too  lazy  to  bother,  try  invoking  SIMTERM  with 
the  -d  flag.  I.e.  say  "simterm  -d"  instead  of  just 
"simterm"..  This  will  keep  SIMTERM  from  looking  at  the  state 
of  your  modem,  and  your  problem  will  disappear.  If  it  still 
doesn't  work,  you  can  always  give  us  a  call. 

12.  Compiling  the  Simulator 

If  you  look  at  the  link  control  file  ' simterm. Ink ' 
you  will  see  the  programs  that  have  to  be  compiled  or 
assembled.  You  must  use  the  MACRO  assembler  (masm) .  The 
file  'struct. mac'  has  some  macros  for  structured  assembly 
code  which  makes  it  easier  to  write.  The  file  ’ mactest . asm ' 
has  some  examples  of  the  use  of  the  structured  macros.  The 
'relations'  between  variable  or  constants  are  the  same  as 
the  conditional  jumps  with  the  'j'  removed,  'long'  can  be 
appended  to  the  macro  to  cause  a  long  jump  to  be  generated. 

'com. asm'  is  the  interface  to  the  Async  Comm  line. 
The  file  'comm.inc'  has  the  entry  points  defined. 
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The  file  ‘comm. inc'  has  the  entry  points  defined. 
* xxgraph. asm '  has  functions  for  cursor  control  and  such  from 
PASCAL  programs.  The  file  ‘graph. inc'  has  the  entry  points 
defined. 


* argl ist . ob j '  and  ‘ argl i st . inc  '  provide  'argv'  and 
* argc '  for  the  parameters  to  the  PASCAL  program.  See 
’ mainsim. pas '  for  examples  of  their  use.  Host  of  the  program 
will  be  self-explanatory  (hopefully).  Any  questions,  give  me 
a  call. 


If  you  want  to  run  SIMTERM  on  a  PC  with  128K,  you 
will  have  to  have  access  to  the  compiler  and  macro 
assembler.  Instead  of  re-compiling  and  linking  the  programs 
in  simterm.lnk,  use  "smterm. Ink".  This  will  build  a  SIMTERM 
without  VenTel  features  and  without  Login  Scripts,  but  it 
also  knocks  about  50K  off  the  ‘.exe'  file. 

13.  Funny  Error  Conditions 

Sometimes  SIMTERM  will  appear  to  lockup  and  not 
accept  any  input  from  the  keyboard  (you  will  hear  the  bell 
indicating  that  the  input  buffer  is  full).  This  is  usually 
caused  by  an  incomplete  ESCAPE  sequence  that  has  been  sent 
to  SIMTERM  and  it  is  waiting  for  the  rest  of  the  characters, 
which  will  probably  never  come  from  the  computer.  To  ‘break 
out'  of  this  condition,  depress  at  the  same  time  the  ‘Ctrl', 
’Shift',  and  ‘Alt'  keys  on  the  left  side  of  the  keyboard. 
This  will  cause  SIMTERM  to  terminate  its  ‘wait  state'  and 
continue.  This  sometimes  happens  after  dialing  into  a  BBS  or 
other  system  since  you  might  get  some  noise  of  the  line. 

Also  the  transmitter  may  have  missed  an  interrupt, 
and  what  you  are  typing  is  not  sent.  If  you  suspect  this, 
hit  A1 t— F7. 


If  SIMTERM  gets  into  a  funny  condition  that  you 
can  not  get  it  out  of,  then  terminate  it  with  ’Alt-Fl'  and 
then  reinvoke  it.  This  procedure  will  leave  the  PC  attached 
to  the  system  and  you  can  pick  up  where  you  left  off. 

There  have  been  problems  on  some  systems  that  use 
PRCJKEY,  NEWKEY,  or  otr.er  software  that  interprets  the 
keyboard.  If  SIMTERM  is  locking  up  on  you  and  you  have  a 
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program  like  this  loaded,  reboot  the  system  without  it  and 
try  it.  Whatever  those  programs  are  doing,  they  are  not  very 
friendly  to  SIMTERM. 

Jim  Holtman  Eric  Holtman 

35  Dogwood  Trail  Watson  Dorm,  Room  142 

Randolph,  N.  J.  07869  Station  #2 

<201 >-361-3395  Chari ottesvi 1 1 e ,VA  22904-0029 

Net  Addresst  . . . ! harpo ! whuxlb ! jph  . . . ! harpo ! whuxle ! eric 

If  you  add  any  interesting  new  features  to  the 
program,  please  keep  me  informed. 

I  have  several  other  public  domain  programs  that  I 
will  be  willing  to  trade  for  ones  that  I  don't  have.  Drop 
me  a  note  (or  a  floppy  disk)  with  what  you  have  and  we  can 
setup  a  trade.  Changes  and  improvements  are  continually 
being  added  to  the  program  and  if  you  are  interested  in  what 
the  changes  are,  just  drop  me  a  note  (or  a  floppy)  for  the 
latest  information. 
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This  Appendix  contains  tha  listings  o-f  the  -file 


"biosio. asm"  and  " dost i me. asm" ,  used  in  the  IBM-PC  Kalman 


***  biosio. asm  *** 


;  ***  biosio. asm  *** 

I 

;  IBM-PC  mi crosof t  "C"  under  PC-DOS  v2.00 

; 

I  MICROSOFT  "C"  callable  8088  assembly  routinss  that 
{interface  dirsctly  with  ths  basic  I/O  systsm  (BIOS). 


I  NOTE  —  Ths  IBM  Tachnical  Reference  Manual  contains  a 
;  listing  o-f  ths  BIOS  and  mors  comp  1st*  dascriptions  of  aach 
; intarrupt. 


I 

;  Written  by  L.  Cuthbertson,  April  1984 


i 

;  **#**##****•»**-**■»■****■»■»•»**•*■»#■»■*#*•»*■»■»***■»•»■»■»■»■*■»**■»#•»■*■»•»■»* 

; 

PGROUP  GROUP  PROG 

PROG  SEGMENT  BYTE  PUBLIC  'PROG* 


PUBLIC  BIOSINI ,BI OSSET , BIOSPOS 

PUBL I C  B I OSUP , B I OSD WN f  B I OSRC A , B I OSWCA , B I OSWC 

PUBLIC  B I QSWD , B I QSTT Y , B I QSCUR 

PUBLIC  COMINI ,COMOUT, COM IN, COMST AT 

PUBLIC  INP,QUTP 


ASSUME  CSx PGROUP 

; 

; *********************************************************** 
;  ***  VIDEO  I/O  *■** 

;  NOTE  -  the  video  I/O  interrupt  description  starts  on  page 
;A-43  of  the  Tech  Ref  Manual. 


;  ****###********«**-»**-»***#****#*-»•»***#*##-»**#**-»-»*#**#***## 


; 


Initialize  screen  I/O  using  the  BIOS  set  mode  call 
synapsis  biasini (stype) ; 

int  stype;  screen  type 

0  *  40x25  BW  (power  on  default) 

1  -  40x25  Color 

2  =  80x25  BW 

3  =  80x25  Color 
graphics  mode 

4  *  320x200  Color 

5  «  320x200  BW 

6  >  640x200  BW 


•V' 

.■*  .■* 
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internal  u««  only 
7  -  80x25  BW  card 


BIOSINZ  PROC  NEAR 
PUSH  BP 
MOV  BP,SP 
MOV  AL, CBP+43  \ 

MOV  AH,0  i 


I  SCREEN  TYPE  IN  AL 
;  SET  MODE  FUNCTION 


INT  10H 
POP  BP 
RET 

BIOSINI  ENDP 


j  BIOS  VIDEO  I/O  INTERRUPT 


Set  the  current  cursor  position. 

synopsis  bi osset (irow,icol ) ; 

***  no  value  returned  *** 
int  irow;  0  to  24 

int  icol;  0  to  79 


B I  OSSET  PROC  NEAR 

PUSH  BP 
MOV  BP,SP 
MOV  DHf  CBP+41  j 

MOV  DL, CBP+61  j 

MOV  BH,0  3 

MOV  AH, 2  3 

INT  10H  ;  VII 

POP  BP 
RET 

B I OSSET  ENDP 


;  ROW 
;  COLUMN 

;  CURRENT  PAGE  NUMBER 
;  CURSOR  POSITION  SET  FUNTION  NUMBER 
VIDEO  I/O  INTERRUPT 


Return  the  current  cursor  post ion. 

synopsis  iret  ■  biosposO) 

int  iret;  high  order  bits  contain  row 
low  order  bits  contain  column 


BIOSPOS  PROC 
PUSH  BP 
MOV  BP,SP 
MOV  BH,0 
MOV  AH ,3 
INT  10H 
MOV  AH,DH 
MOV  AL,DL 
POP  BP 
RET 

BIOSPOS  ENDP 


NEAR 


;  CURRENT  PAGE  NUMBER 
;  CURSOR  POSITION  FUNCTION  NUMBER 
VIDEO  I/O  INTERRUPT 

;  MOVE  INT  RETURN  INTO  FUNCTION  RETURN 
;  DITTO 


Read  the  contents  of  a  given  screen  cell. 

synopsi s  i ret »b i osr ca ( ) ; 

int  iret;  high  order  bits  contain  attributes 
low  order  bits  contain  character 


I  NOTE  -  Attributes  are  defined  on  page  13-9  of  the  DOS  v2.0 
;  manual 

BIOSRCA  PROC  NEAR 

PUSH  BP 
MOV  BP, SP 

MOV  BH,0  5  ACTIVE  DISPLAY  PAGE 

MOV  AH, 8  |  READ  CHARACTER  +  ATTRIBUTES  FUNCTION 

INT  1 OH  ;  VIDEO  I/O  INTERRUPT 

POP  BP 
RET 

BIOSRCA  ENDP 


■ 

* 

;  Writs  a  character  to  the  screen  -  with  attributes. 


■ 

9 

;  synapsis  biaswea (char , count ,att> ; 


• 

9 

m 

9 

; 

; 

; 


***  no  value  returned  *** 

int  char;  character  to  output 

int  count;  number  of  times  to  output  character 

int  att;  character  attribute 


;  NOTE  -  Attributes  are  defined  on  page  13-9  of  the  DOS  v2.0 
; manual 


BIOSWCA  PROC  NEAR 

PUSH  BP 
MOV  BP,SP 

MOV  AL, CBP+41  ;  CHARACTER 

MOV  CX , CBP+63  ;  NUMBER  OF  CHARACTERS  TO  WRITE 
MOV  BL, CBP+8I  ;  CHARACTER  ATTRIBUTE 
MOV  BH,0  ;  ACTIVE  DISPLAY  PAGE 

MOV  AH, 9  ;  WRITE  CHARACTER/ w  ATTRIBUTES  FUNCTION 

INT  10H  ;  VIDEO  I/O  INTERRUPT 

POP  BP 
RET 

BIOSWCA  ENDP 


? 

;  Write  a  character  to  the  screen  -  no  attributes. 


9 


synopsis  bioswc (char .count ) : 


***  no  value  returned  *** 

int  charf  charactar  to  output 

int  count;  number  of  times  to  output  charactar 


BIOSWC  PRDC  NEAR 

PUSH  BP 
MOV  BP, SP 
MOV  AL, CBP+43  | 

MOV  CX , CBP+63  ; 

MOV  BL,0;  ; 

MOV  BH,0  i 

MOV  AH, 10  j 

INT  10H  ;  VII 

POP  BP 
RET 

BIOSWC  ENDP 


|  CHARACTER 

|  NUMBER  OF  CHARACTERS  TO  WRITE 
|  CHARACTER  ATTRIBUTE  -  NULL 
;  ACTIVE  DISPLAY  PAGE 

j  WRITE  CHARACTER  ONLY  FUNCTION  NUMBER 
VIDEO  I/O  INTERRUPT 


Write  a  dot  in  graphics  mode. 

synopsis  bioswd (irow, icol ) ; 

*•**  no  value  returned  *** 
int  irow; 
int  icol; 


BIOSWD  PROC  NEAR 

PUSH  BP 
MOV  BP,SP 
MOV  DX , CBP+43  5 

MOV  CX , CBP+63  ; 

MOV  AL, 1  5 

MOV  AH, 12  5 

INT  10H  ;  VII 

POP  BP 
RET 

BIOSWD  ENDP 


;  ROW 
;  COLUMN 
;  GREEN  COLOR 

;  WRITE  A  DOT  FUNCTION  NUMBER 
VIDEO  I/O  INTERRUPT 


;  Return  the  current  video  state  of  the  screen. 

;  synopsis  iret  -  bioscurO; 

;  int  iret;  low  bits  are  the  mode  currently  set 

;  (see  biosini  for  description) 

;  high  bits  are  the  number  of  columns  on 

screen 


BIOSCUR  PROC  NEAR 
PUSH  BP 
MOV  BP,SP 


MOV  AH, 15  I  CURRENT  VIDEO  STATE  FUNCTION 

INT  10H  j  BIOS  VIDEO  I/O  INTERRUPT 

POP  BP 
RET 

BIOSCUR  ENDP 

9 

l  #***#*#*#****«*###*#***##****##***#*###***-»**#****#**##*##* 
9 

I  ***  communications  port  *■»* 

;  NOTE  -  ths  communications  port  I/O  is  described  starting 
ion  page  A-20  of  the  Tech  Ref  Manual. 

j  a********************************************************** 


;  Initialize  the  communications  port. 

;  synopsis  iret  =*  comini  (port , params) ; 

;  int  iret |  return  status  (see  comstat) 

;  int  part;  communications  port  to  initialize  (0,1) 

;  int  params;  bit  pattern  for  initialization  - 


7  6  5  4 

-BAUD  RATE- 

000  -  110 
OOl  -  150 
010  -  300 
Oil  -  600 
100  -  1200 
101  -  2400 

110  -  4800 

111  -  9600 


COMINI  PROC  NEAR 
PUSH  BP 
MOV  BP'SP 
MOV  DX ' CBP+43 
MOV  AL.CBP+63 
MOV  AH'O 


3  2  10 

-PARITY-  -STOP  BIT - WORD  LENGTH- 

XO  -  NONE  0-1  10-7  BITS 
01  -  ODD  1  -  2  11  -  8  BITS 
11  -  EVEN 


COMM  PORT  TO  INITIALIZE 
PARAMETERS 

INITIALIZATION  FUNCTION 


INT  14H 
POP  BP 
RET 

COMINI  ENDP 


;  COMM  PORT  I/O  INTERRUPT 


;  Write  a  character  to  the  communications  port. 

; 

;  synapsis  iret  ■  comout (port ,c) ; 

S 


i  int  iret;  error  return  (  >127  if  error  occur ed) 

;  -  see  AH  under  modem  control 

;  int  port}  communications  port  to  writs  (0  or  1) 

I  int  c|  charactsr  to  writs. 

I 

C0M0UT  PROC  NEAR 

PUSH  BP 
MOV  BP, SP 

MOV  DX , CBP+43  j  COMM  PORT  TO  WRITE 

MOV  AL, CBP+63  {  CHARACTER  TO  WRITE 

MOV  AH, 1  {WRITE  CHARACTER  FUNCTION 

INT  14H  {  COMM  PORT  I/O  INTERRUPT 

POP  BP 
RET 

COMOUT  ENDP 

9 

t  Read  a  charactsr  from  ths  communications  port. 

;  Waits  for  charactsr  if  ons  is  not  ready. 

;  Sss  \comm\ibmtty.c  for  example  of  polling  comm  port 
;  for  character  without  wait. 

5 

;  synopsis  c  »  comin(port); 

■ 

9 

;  int  c;  character  read  from  comm  port, 

;  >  127  if  error  or  no  character  ready. 

;  -  see  AH  under  modem  control 

;  int  port{  communications  port  to  read  (0  or  1) 

l 

COM IN  PROC  NEAR 

PUSH  BP 
MOV  BP,SP 

MOV  DX , CBP+43  j  COMM  PORT  TO  READ 
MOV  AH, 2  ;  READ  CHARACTER  FUNCTION 

INT  14H  ;  COMM  PORT  I/O  INTERRUPT 

POP  BP 
RET 

COM IN  ENDP 

■ 

9 

{  Check  the  line  and  modem  status 
f 

;  synopsis  iret  *  comstat (port ) ; 

a 

» 

;  int  iret{  ;  line  and  modem  status 

{  AH  -  high  order  bits  contain  line  control  status 

;  bit  7  ■  time  out 

;  bit  6  ■  trans  shift  register  empty 

;  bit  5  *  trans  holding  register  empty 

;  bit  4  *  break  detect 

;  bit  3  *  framing  error 

{  bit  2  ■  parity  error 


I  bit  1  *  overrun  mrrar 

;  bit  0  ■  data  ready 

■ 

» 

;  AL  -  low  order  bits  contain  modem  status 

;  bit  7  ■  received  line  signal  detect 

;  bit  &  ■  ring  indicator 

;  bit  5  -  data  set  ready 

;  bit  4  *  clear  to  send 

;  bit  3  ■  delta  receive  line  signal  detect 

;  bit  2  >  trailing  edge  ring  detector 

;  bit  1  ■  delta  data  set  ready 

;  bit  0  a  delta  clear  to  send 

■ 

9 

i  Note  -  from  page  A-21  of  Tech  Ref  Manual 

m 

9 

\  int  port;  ;  communications  port  to  check 

■ 

CCJMSTAT  PROC  NEAR 

PUSH  BP 
MOV  BP,SP 

MOV  DX , CBP+43  ;  COMM  PORT  TO  CHECK 
MOV  AH, 3  ;  STATUS  FUNCTION 

INT  14H  ;  COMM  PORT  I/O  INTERRUPT 

POP  BP 
RET 

COMSTAT  ENDP 

; 

; ***#***#**#****#***********#*##*******##**#*###*##«##*#-»#*# 
• 

9 

;  *■**  io. asm  #** 

9 

t  IBM -PC  S088  assembly  for  interface  with  microsoft  "C" 

; under  PC-DOS 

m 

9 

;  "C"  callable  subroutines  that  provide  absolute  pointer 
; addressing. 

;  Pointers  within  a  IBM— PC  microsoft  "C"  program  are 
; relative  to  the  start  of  the  programs  data  segment.  These 
subroutines  provide  a  mechanism  to  address  absolute  memory 
; locations. 

■ 

* 

;  Supplied  by  microsoft  -  commented  by  L.  Cuthbertson,  April 

,*1984 

5 

;  *********************************.*#***#******.************** 


Read  an  absolute  memory  location, 
synopsis  c  =  readabs(loc); 


'V 


I  int  c; 

;  int  lac | 

INP  PROC  NEAR 

PUSH  BP 

MOV  BP,SP 
MOV  DX , CBP+43 
IN  AL,DX 
XOR  AH, AH 
POP  BP 
RET 

INP  ENDP 


con  tan  ts  of  memory  location 
absolute  memory  location  in  hex 


; GET  LOCATION  ADDRESS 
; READ  LOCATION 
; CLEAR  HIGH  BYTE 


Write  an  absolute  memory  location. 

synopsis  writ sabs (loc,c) ; 

int  loc;  absolute  memory  location  in  hex 
int  c;  integer  to  write  to  memory  location 


NEAR 


OUTP  PROC  N 

PUSH  BP 
MOV  BP, SP 
MOV  DX, CBP+43 
MOV  AX , CBP+63 
OUT  DX , AL 
POP  BP 
RET 

OUTP  ENDP 


♦♦a*#****#**************************#****-****************** 


; GET  LOCATION  ADDRESS 
; GET  ADDRESS  OF  INTEGER  TD  WRITE 
; WRITE  TO  MEMORY  LOCATION 


PROG  ENDS 
END 


***  dostime.asm  *** 


IBM-PC  microsoft  "C"  under  PC-DOS 

Microsoft  "C"  callable  8088  assembly  routine  that  uses  a 
DOS  function  call  to  obtain  and  return  the  hours,  minutes, 
seconds,  and  hundreths  of  a  second  as  integers. 

synapsis  dostime(&hour ,&min,&sec, &ths> ; 

#**  no  function  value  returned  **■* 
int  hour;  0-23  military  time 

int  min;  0-59 

int  sec;  0-59 

I  int  ths;  0-99 


/*/  'V.*  v  V  V  V  *.*  *  *  *  •  'v  »v  »'  >  .**  . 


I 

I  Written  by  L.  Cuthbartson,  May  1984 
1 

J  a##*#*##*###**##****##**##*##***#*#*##*******#*******#*#*** 
I 

PGROUP  GROUP  PROG 

PROG  SEGMENT  BYTE  PUBLIC  'PROG' 

PUBLIC  DOSTIME 
ASSUME  CS i PGROUP 
i 

; *********************************************************** 
DOSTIME  PROC  NEAR 


PUSH  BP 

MOV 

BP,SP 

MOV 

AH,2CH 

■ 

f 

DOS  GET  TIME  FUNCTION  NUMBER 

INT 

21H  ; 

DOS 

FUNCTION  CALL  INTERRUPT 

MOV 

AL,CH 

; 

MOVE  HOURS  INTO  AX 

XOR 

AH,  AH 

• 

9 

CLEAR  HIGH  BYTE 

MOV 

DI , CBP+4D 

\ 

ADDRESS  OF  HOURS  VARIABLE 

MOV 

CDI3,AX 

m 

9 

MOVE  HOURS  INTO  HOURS  VARIABLE 

MOV 

BL,CL 

■ 

9 

MOVE  MINUTES  INTO  BX 

XOR 

BH,BH 

■ 

9 

CLEAR  HIGH  BYTE 

MOV 

DI , CBP+63 

■ 

9 

ADDRESS  OF  MINUTES  VARIABLE 

MOV 

EDI ] ,BX 

9 

9 

MOVE  MINUTES  INTO  MINUTES  VARIABLE 

MOV 

CL,DH 

■ 

5 

MOVE  SECONDS  INTO  CX 

XOR 

CH,CH 

; 

CLEAR  HIGH  BYTE 

MOV 

DI , CBP+83 

i 

ADDRESS  OF  SECONDS  VARIABLE 

MOV 

EDI  3 ,CX 

j 

MOVE  SECONDS  INTO  SECONDS  VARIABLE 

XOR 

DH,DH 

• 

9 

CLEAR  HIGH  BYTE 

MOV 

DI,EBP+103 

9 

9 

ADDRESS  OF  THS  VARIABLE 

MOV 

EDI  3 ,DX 

1 

MOVE  1/lOOTHS  INTO  THS  VARIABLE 

POP 

BP 

RET 

DOST I ME 

ENDP 

PROG 

ENDS 

END 
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